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

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.

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

Binary file not shown.

46
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/setup_turtlebot.md Executable file
View File

@@ -0,0 +1,46 @@
Hostname des Turtlebots: turtlebot
Password: turtlebot3
Setup:
Zunächst muss sich der Turtlebot im gleichen Netz wie der Remote-PC befinden. Im Auslieferungszustand wird sich der EC700-BT Rechner in ein WLAN mit der SSID "Ubuntu-Hotspot" mit dynamischer IP einloggen, da dies meine Konfiguration war.
Die Empfehlung ist, bei der ersten Inbetriebnahme Maus und Tastatur am hinteren Ende des AGC, sowie HDMI-Kabel vorne neben dem OLS20 in den Rechner einzustecken. Dann kann manuell ein WLAN-Netzwerk gespeichert werden, in welches sich der Rechner nach dem booten einloggt (vorzugsweise mit fester IP-Adresse).
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).
Nachdem sichergestellt ist, dass sich der Turtlebot-PC beim Neustart in das gewünschte Netz einloggt, kann Maus, Tastatur und HDMI-Kabel abgezogen werden und der Turtlebot-PC neu gestartet werden.
Damit im folgenden Abschnitt nicht bei jeder ssh-Verbindung das Password eingegeben werden muss, empfielt es sich, am Remote-PC ein rsa-key zu generieren und diesen auf dem Turtlebot-PC zu speichern. Dazu auf dem Remote-PC folgende Befehle ausführen:
$ ssh-keygen -t rsa #Den Key ohne Password durch zweimaliges drücken von Enter speichern.
$ ssh-copy-id -i ~/.ssh/id_rsa.pub turtlebot@Turtlebot-IP-Adresse
Starten der Robot-Fsm:
1. Turtlebot an Powerbank einstecken und ca. 1 min warten, bis sich der Turtlebot-PC in das konfigurierte Netzwerk eingeloggt hat.
2. roscore vom Remote-PC aus starten.
3. Konsolenfenster öffnen und eine ssh-Verbindung zum Turtlebot-PC erstellen. Anschließend zum OpenCR-Board verbinden. Dies geschieht Normalerweise durch den Befehl "roslaunch turtlebot3_bringup turtlebot3_robot.launch". Der Befehl ist jedoch durch den Kurzbefehl "rl" hinterlegt. Somit folgende Befehle ausführen:
$ ssh turtlebot@IP-Adresse #Danach besteht das Konsolenfenster auf dem Turtlebot-PC
$ rl # Am Anfang wird ein Fehler auftauchen, da der Standard-Laserscanner nicht verbunden ist. Der Fehler kann ignoriert werden.
4. Konsolenfenster öffnen und eine ssh-Verbindung zum Turtlebot-PC erstellen. Anschließend den ols-Treiber starten. Dies geschieht Normalerweise durch den Kurzbefehl "ols". Dieser konfiguriert die can0-Schnittstelle und führt das sick_line_guidance launch-file mit dem ols-yaml-file aus. Somit folgende Befehle ausführen:
$ ssh turtlebot@IP-Adresse #Danach besteht das Konsolenfenster auf dem Turtlebot-PC
$ ols
5. Die gpio-handler Node vom Remote-PC aus starten:
$ rosrun gpio_handling gpio_handler
### ACHTUNG: Beim ausführen der nächsten Befehle wird sich der Turtlebot geradeaus bewegen. ###
6. Das yaml-Parameter-File für die robot-fsm-Node laden und anschließend die Node starten:
$ rosparam load ~/catkin_ws/src/iam/yaml/AGC.yaml
$ rosrun iam
7. Durch starten der Teleop-Node kann der Turtlebot gestoppt werden.
(Achtung hier publishen nun zwei Nodes auf dem gleichen Topic (cmd_vel). Deshalb muss die robot_fsm Node kurz nach dem starten der teleop-Node mit strg+c beendet werden):
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch # Durch drücken der Taste s auf der Tastatur, kann der Turtlebot sofort gestoppt werden.
Der Turtlebot fährt nun geradeaus. Wird eine Linie erkannt erfolgt ein Wechsel in den State follow_line. Der Turtlebot folgt der Linie. Geht die Linie verloren, wird der Turtlebot nach 1 Sekunde stoppen.

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

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 32 KiB

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

@@ -0,0 +1,53 @@
cmake_minimum_required(VERSION 2.8.3)
project(gpio_handling)
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
std_msgs
)
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
add_service_files(
FILES
gpio_set_config.srv
gpio_get_config.srv
gpio_set_pin.srv
gpio_get_pin.srv
)
generate_messages(
DEPENDENCIES
std_msgs
custom_messages
)
#catkin_package(
# # INCLUDE_DIRS include
# LIBRARIES gpio_handling
# CATKIN_DEPENDS message_generation message_runtime roscpp std_msgs
#)
include_directories( ${catkin_INCLUDE_DIRS} )
# Service Server
add_executable(gpio_handler src/gpio_handler.cpp)
add_dependencies(gpio_handler ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(gpio_handler ${catkin_LIBRARIES} )
install(TARGETS gpio_handler
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)

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

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

34
Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/package.xml Executable file
View File

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

82
Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.cpp Executable file
View File

@@ -0,0 +1,82 @@
#include "gpio_handler.h"
int main(int argc, char **argv) // node main function
{
ros::init(argc, argv, "gpio_handler"); // initialize node with given name
ROS_INFO("Node %s started", ros::this_node::getName().c_str()); // print info to ros command line
ros::NodeHandle nh; // init node handle
// init servive servers
ros::ServiceServer set_config_server = nh.advertiseService("gpio_set_config", gpio_set_config);
ROS_INFO("gpio set_config_server ready. Call it with: rosservice call %s", set_config_server.getService().c_str());
ros::ServiceServer get_config_server = nh.advertiseService("gpio_get_config", gpio_get_config);
ROS_INFO("gpio get_config_server ready. Call it with: rosservice call %s", get_config_server.getService().c_str());
ros::ServiceServer set_server = nh.advertiseService("gpio_set_pin", gpio_set_pin);
ROS_INFO("gpio set_server ready. Call it with: rosservice call %s", set_server.getService().c_str());
ros::ServiceServer get_server = nh.advertiseService("gpio_get_pin", gpio_get_pin);
ROS_INFO("gpio get_server ready. Call it with: rosservice call %s", get_server.getService().c_str());
// init publisher and subscriber
ros::Publisher set_publisher = nh.advertise<custom_messages::gpio>("/gpio_set", 1, false);
ros::Subscriber get_subscriber = nh.subscribe("/gpio_get", 1, gpioCallback);
/* gpio-message default values */
for (int i = 0; i <= 17; i++)
{
gpio_set_msg.pin_config[i] = 4; //input pulldown (high impedance)
gpio_set_msg.pin_value[i] = 0;
gpio_get_msg.pin_value[i] = 4;
gpio_get_msg.pin_value[i] = 0;
}
ros::Rate loop_rate(200); // publish message rate = 200 Hz
while(ros::ok())
{
ros::spinOnce(); // handle service requests and subscriber callbacks
set_publisher.publish(gpio_set_msg); // publish messages
loop_rate.sleep(); // sleep for loop_rate
}
return 0;
}
void gpioCallback(const custom_messages::gpioConstPtr &msg){
gpio_get_msg = *msg;
}
bool gpio_set_config(gpio_handling::gpio_set_config::Request &req, gpio_handling::gpio_set_config::Response &res){
for(uint8_t i = 0; i < req.pinNumber.size(); i++)
{
gpio_set_msg.pin_config[req.pinNumber[i]] = req.pinMode[i];
}
return true;
}
bool gpio_get_config(gpio_handling::gpio_get_config::Request &req, gpio_handling::gpio_get_config::Response &res){
res.pinMode.resize(req.pinNumber.size());
for(uint8_t i = 0; i < req.pinNumber.size(); i++)
{
res.pinMode[i] = gpio_get_msg.pin_config[req.pinNumber[i]];
}
return true;
}
bool gpio_set_pin(gpio_handling::gpio_set_pin::Request &req, gpio_handling::gpio_set_pin::Response &res){
for(uint8_t i = 0; i < req.pinNumber.size(); i++)
{
gpio_set_msg.pin_value[req.pinNumber[i]] = req.pinValue[i];
}
return true;
}
bool gpio_get_pin(gpio_handling::gpio_get_pin::Request &req, gpio_handling::gpio_get_pin::Response &res){
res.pinValue.resize(req.pinNumber.size());
for(uint8_t i = 0; i < req.pinNumber.size(); i++)
{
res.pinValue[i] = gpio_get_msg.pin_value[req.pinNumber[i]];
}
return true;
}

19
Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/src/gpio_handler.h Executable file
View File

@@ -0,0 +1,19 @@
#include "ros/ros.h"
#include "custom_messages/gpio.h"
#include "gpio_handling/gpio_set_config.h"
#include "gpio_handling/gpio_get_config.h"
#include "gpio_handling/gpio_set_pin.h"
#include "gpio_handling/gpio_get_pin.h"
#include <string>
#include <iostream>
// messages for publish and subscribing for communication to openCR-board
custom_messages::gpio gpio_set_msg;
custom_messages::gpio gpio_get_msg;
// function Declaration
void gpioCallback(const custom_messages::gpioConstPtr &msg);
bool gpio_set_config(gpio_handling::gpio_set_config::Request &req, gpio_handling::gpio_set_config::Response &res);
bool gpio_get_config(gpio_handling::gpio_get_config::Request &req, gpio_handling::gpio_get_config::Response &res);
bool gpio_set_pin(gpio_handling::gpio_set_pin::Request &req, gpio_handling::gpio_set_pin::Response &res);
bool gpio_get_pin(gpio_handling::gpio_get_pin::Request &req, gpio_handling::gpio_get_pin::Response &res);

3
Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_config.srv Executable file
View File

@@ -0,0 +1,3 @@
uint8[] pinNumber
---
uint8[] pinMode

3
Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_get_pin.srv Executable file
View File

@@ -0,0 +1,3 @@
uint8[] pinNumber
---
bool[] pinValue

3
Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_config.srv Executable file
View File

@@ -0,0 +1,3 @@
uint8[] pinNumber
uint8[] pinMode
---

3
Devices/Packages/sick_line_guidance/turtlebotDemo/gpio_handling/srv/gpio_set_pin.srv Executable file
View File

@@ -0,0 +1,3 @@
uint8[] pinNumber
bool[] pinValue
---

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

@@ -0,0 +1,51 @@
cmake_minimum_required(VERSION 2.8.3)
project(lidar_obstacle_detection)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++17)
find_package(catkin REQUIRED COMPONENTS
message_generation
roscpp
std_msgs
)
add_message_files(
FILES
obstacle_detector_msg.msg
)
add_service_files(
FILES
obstacle_detector_config.srv
)
# Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
#catkin_package(
# #INCLUDE_DIRS include
# LIBRARIES lidar_obstacle_detection
# CATKIN_DEPENDS message_generation roscpp std_msgs message_runtime
# #DEPENDS system_lib
#)
###########
## Build ##
###########
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(obstacle_detector src/obstacle_detector.cpp)
add_dependencies(obstacle_detector ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(obstacle_detector ${catkin_LIBRARIES})
add_executable(obstacle_detector_config src/obstacle_detector_config.cpp)
add_dependencies(obstacle_detector_config ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(obstacle_detector_config ${catkin_LIBRARIES})

8
Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/msg/obstacle_detector_msg.msg Executable file
View File

@@ -0,0 +1,8 @@
Header header
bool obsctacle_stop
float32 obstacle_stop_range
int16 angle_min
int16 angle_max
float32 range_min
float32 range_max
float32 object_distance_min

31
Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/package.xml Executable file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<package format="2">
<name>lidar_obstacle_detection</name>
<version>0.0.0</version>
<description>The lidar_obstacle_detection 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_depend>message_runtime</build_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

97
Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector.cpp Executable file
View File

@@ -0,0 +1,97 @@
#include "ros/ros.h"
#include "lidar_obstacle_detection/obstacle_detector_msg.h" // for publishing
#include "lidar_obstacle_detection/obstacle_detector_config.h" // for service
#include "sensor_msgs/LaserScan.h" // for subscribing
#include <string>
#include <iostream>
#include <sstream>
lidar_obstacle_detection::obstacle_detector_msg obstacle_detector_msg;
void scan_Callback(const sensor_msgs::LaserScan::ConstPtr& msg){
uint16_t angle;
obstacle_detector_msg.obsctacle_stop = false;
obstacle_detector_msg.object_distance_min = obstacle_detector_msg.range_max;
//ROS_INFO("angle_min: %d angle_max: %d", obstacle_detector_msg.angle_min, obstacle_detector_msg.angle_max);
for(int16_t i = obstacle_detector_msg.angle_min; i < obstacle_detector_msg.angle_max; i++){
if(i < 0)
{
angle = i+360;
}
else
{
angle = i;
}
if(msg->ranges[angle] > obstacle_detector_msg.range_min && msg->ranges[angle] < obstacle_detector_msg.range_max) // if range value is between range_min and range_max
{
//ROS_INFO("in range value at angle: %d range: %f", angle, msg->ranges[angle]);
if(msg->ranges[angle] < obstacle_detector_msg.object_distance_min)
{
obstacle_detector_msg.object_distance_min = msg->ranges[angle];
}
if(msg->ranges[angle] < obstacle_detector_msg.obstacle_stop_range)// object in stop range
{
obstacle_detector_msg.obsctacle_stop = true;
}
}
}
}
bool config_Service_callback(lidar_obstacle_detection::obstacle_detector_config::Request &req,
lidar_obstacle_detection::obstacle_detector_config::Request &res){
obstacle_detector_msg.range_min = req.range_min;
ROS_INFO("set range_min to: %f", req.range_min);
obstacle_detector_msg.range_max = req.range_max;
ROS_INFO("set range_max to: %f", req.range_max);
obstacle_detector_msg.angle_min = req.angle_min;
ROS_INFO("set angle_min to: %d", req.angle_min);
obstacle_detector_msg.angle_max = req.angle_max;
ROS_INFO("set angle_max to: %d", req.angle_max);
obstacle_detector_msg.obstacle_stop_range = req.obstacle_stop_range;
ROS_INFO("set obstacle_stop_range to: %f", req.obstacle_stop_range);
return true;
}
int main(int argc, char **argv){
//defaults
obstacle_detector_msg.angle_min = -30;
obstacle_detector_msg.angle_max = 30;
obstacle_detector_msg.range_min = 0.2;
obstacle_detector_msg.range_max = 0.5;
obstacle_detector_msg.obstacle_stop_range = 0.3;
obstacle_detector_msg.header.frame_id = "obstacle_detector_frame";
ros::init(argc, argv, "obstacle_detector");
ros::NodeHandle nh;
ROS_INFO("Node %s started.", ros::this_node::getName().c_str());
char buffer [128];
sprintf(buffer, "%s/obstacle_detector_config", ros::this_node::getNamespace().c_str());
ros::ServiceServer config_server = nh.advertiseService(buffer, config_Service_callback);
ROS_INFO("Service Server for configuration started. Call it with \"rosservice call %s\"", config_server.getService().c_str());
sprintf(buffer, "%s/scan", ros::this_node::getNamespace().c_str());
ros::Subscriber scan_sub = nh.subscribe<sensor_msgs::LaserScan>(buffer, 1, scan_Callback);
ROS_INFO("Subscribing topic %s", scan_sub.getTopic().c_str());
sprintf(buffer, "%s/obstacle", ros::this_node::getNamespace().c_str());
ros::Publisher obstacle_detector_pub = nh.advertise<lidar_obstacle_detection::obstacle_detector_msg>(buffer, 1);
ROS_INFO("Publishing topic %s", obstacle_detector_pub.getTopic().c_str());
ros::Rate loop_rate(100); // Publiziere Nachricht mit 100 Hz
while(ros::ok())
{
obstacle_detector_msg.header.stamp = ros::Time::now();
obstacle_detector_pub.publish(obstacle_detector_msg);
ros::spinOnce();
loop_rate.sleep(); // Sleep for 100 ms
}
return 0;
}

43
Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/src/obstacle_detector_config.cpp Executable file
View File

@@ -0,0 +1,43 @@
#include "ros/ros.h"
#include "lidar_obstacle_detection/obstacle_detector_config.h"
int main(int argc, char **argv) // Node Main Function
{
ros::init(argc, argv, "obstacle_detector_config"); // Initializes Node Name
if (argc != 5)
{
ROS_INFO("cmd : rosrun own own_client arg0 arg1 arg2 arg3 arg4");
ROS_INFO("arg0: angle_min, type: int16 \n arg1: angle_max, type: int16 \n arg2: range_min, type: int16 \n arg3: rang_max, type: int16 \n arg4: obstacle_stop_range, type: int16 \n");
return 1;
}
char buffer [128];
sprintf(buffer, "%s/obstacle_detector_config", ros::this_node::getNamespace().c_str());
ros::NodeHandle nh;
ros::ServiceClient config_client = nh.serviceClient<lidar_obstacle_detection::obstacle_detector_config>(buffer);
// Declares the 'srv' service that uses the 'SrvTutorial' service file
lidar_obstacle_detection::obstacle_detector_config srv;
// Parameters entered when the node is executed as a service request value are stored at 'a' and 'b'
srv.request.range_min = atoll(argv[1]);
srv.request.range_max = atoll(argv[2]);
srv.request.angle_min = atoll(argv[3]);
srv.request.angle_max = atoll(argv[4]);
srv.request.obstacle_stop_range = atoll(argv[5]);
config_client.call(srv);
/*
// Request the service. If the request is accepted, display the response value
if (config_client.call(srv))
{
ROS_INFO("send srv, srv.Request.in: %s", srv.request.in.c_str());
ROS_INFO("receive srv, srv.Response.result: %s", srv.response.out.c_str());
}
else
{
ROS_ERROR("Failed to call service own_service");
return 1;
}
*/
return 0;
}

6
Devices/Packages/sick_line_guidance/turtlebotDemo/lidar_obstacle_detection/srv/obstacle_detector_config.srv Executable file
View File

@@ -0,0 +1,6 @@
float32 range_min
float32 range_max
int16 angle_min
int16 angle_max
float32 obstacle_stop_range
---

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

@@ -0,0 +1,284 @@
cmake_minimum_required(VERSION 2.8.3)
project(sick_line_guidance_demo)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++17 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs -Wno-unused-parameter)
## 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
sick_line_guidance
message_generation
random_numbers
roscpp
rospy
sensor_msgs
std_msgs
nav_msgs
sm_timing
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(TinyXML REQUIRED)
find_package(OpenCV 3 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
# Measurement1.msg
# Measurement2.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
# nav_msgs
# sick_line_guidance
# )
################################################
## 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_demo_lib
# CATKIN_DEPENDS sick_line_guidance message_runtime random_numbers roscpp rospy sensor_msgs std_msgs nav_msgs
# DEPENDS TinyXML
#)
###########
## 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}
${OpenCV_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME}_lib
src/adjust_heading.cpp
src/barcodes.cpp
src/explore_line_state.cpp
src/figure_eight_fsm.cpp
src/follow_line_state.cpp
src/image_util.cpp
src/navigation_mapper.cpp
src/navigation_util.cpp
src/ols_measurement_simulator.cpp
src/pid.cpp
src/robot_fsm.cpp
src/stop_go_fsm.cpp
src/turtlebot_test_fsm.cpp
src/velocity_ctr.cpp
src/wait_at_barcode_state.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(sick_line_guidance_demo_node src/sick_line_guidance_demo_node.cpp)
add_executable(sick_line_guidance_watchdog src/sick_line_guidance_watchdog.cpp)
add_executable(turtlebot_test_fsm_node src/turtlebot_test_fsm_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}_lib
sick_line_guidance_lib
random_numbers
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(sick_line_guidance_demo_node
sick_line_guidance_lib
random_numbers
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(sick_line_guidance_watchdog
sick_line_guidance_lib
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(turtlebot_test_fsm_node
sick_line_guidance_lib
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
## Specify libraries to link a library or executable target against
target_link_libraries(sick_line_guidance_demo_node
random_numbers
sick_line_guidance_lib
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
${OpenCV_LIBRARIES}
profiler
)
target_link_libraries(sick_line_guidance_watchdog
sick_line_guidance_lib
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
${OpenCV_LIBRARIES}
profiler
)
target_link_libraries(turtlebot_test_fsm_node
sick_line_guidance_lib
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
${OpenCV_LIBRARIES}
profiler
)
#############
## 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 sick_line_guidance_demo_node sick_line_guidance_watchdog turtlebot_test_fsm_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/Robot_FSM_demo.launch
launch/sick_line_guidance_demo_node.launch
launch/sick_line_guidance_watchdog.launch
launch/turtlebot_test_fsm_node.launch
maps/cam_intrinsic.xml
maps/demo_barcodes.xml
maps/demo_map_02.png
yaml/sick_line_guidance_demo.yaml
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)

283
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/adjust_heading.h Executable file
View File

@@ -0,0 +1,283 @@
/*
* AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time.
* It searches the line orientation by minimizing the line distance and adjusts the robot heading.
*
* Algorithm:
* - Stop the robot, set linear velocity to 0.0
* - Minimize the line distance by rotating the robot:
* + Start rotating clockwise
* + If the lined distance increases, rotate anti-clockwise
* + Stop rotation when the line distance is minimal.
* The robot is now orientated parallel to the line.
* - Reset PID and continue moving along the line.
*
* 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 __ROBOT_FSM_ADJUST_HEADING_H_INCLUDED
#define __ROBOT_FSM_ADJUST_HEADING_H_INCLUDED
#include <opencv2/core.hpp>
#include "sick_line_guidance_demo/regression_1d.h"
#include "sick_line_guidance_demo/velocity_ctr.h"
#include "sick_line_guidance_demo/time_format.h"
namespace sick_line_guidance_demo
{
/*
* struct AdjustHeadingConfig collects the configuration and parameter for class AdjustHeading:
*/
typedef struct AdjustHeadingConfigStruct
{
AdjustHeadingConfigStruct() : search_lower_bound_linedistance(0), search_upper_bound_linedistance(0), search_lower_bound_linewidth(0), search_upper_bound_linewidth(0), angular_z(0),
measurement_jitter(0), delta_angle_epsilon(0), sigma_linedistance(0), sigma_linewidth(0) {}
double search_lower_bound_linedistance; // lower limit for line distance: if the current line distance is below search_lower_bound_linedistance, the search will revert direction
double search_upper_bound_linedistance; // upper limit for line distance: if the current line distance is above search_upper_bound_linedistance, the search will revert direction
double search_lower_bound_linewidth; // lower limit for line width: if the current line width is below search_lower_bound_linewidth, the search will revert direction
double search_upper_bound_linewidth; // upper limit for line width: if the current line width is above search_upper_bound_linewidth, the search will revert direction
double search_max_yaw_angle_delta; // upper limit for yaw angle: if abs. difference between current yaw_angle and yaw angle at start, the search will revert direction
double angular_z; // velocity.angular.z under adjustment, default: 0.1 * M_PI / 4
double measurement_jitter; // tolerate some line measurement jitter when adjusting the heading, default: 0.003
double delta_angle_epsilon; // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon, default: 0.1 * M_PI / 180
double sigma_linedistance; // standard deviation of line distance, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2)
double sigma_linewidth; // standard deviation of line width, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2)
double max_state_duration; // test only: max time amount in seconds to stay in current search state; after <max_state_duration> seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions
} AdjustHeadingConfig;
/*
* class AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time.
* It searches the line orientation by minimizing the line distance and adjusts the robot heading.
* Algorithm:
* - Stop the robot, set linear velocity to 0.0
* - Minimize the line distance by rotating the robot:
* + Start rotating clockwise
* + If the lined distance increases, rotate anti-clockwise
* + Stop rotation when the line distance is minimal.
* The robot is now orientated parallel to the line.
* - Reset PID and continue moving along the line.
*/
class AdjustHeading
{
public:
/*
* Constructor
*/
AdjustHeading();
/*
* Destructor
*/
virtual ~AdjustHeading();
/*
* Starts a new adjustment.
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry
* @param[in] use_line_width: minimize line distance (false,default) or line width (true)
* @param[in] adjust_heading_cfg Parameter (upper and lower bounds) configuring the search
*/
virtual void start(double line_distance, double line_width, double yaw_angle, bool use_line_width, const AdjustHeadingConfig & adjust_heading_cfg);
/*
* Stops a running adjustment.
*/
virtual void stop(void);
/*
* Returns true, if an adjustment is currently running.
*/
virtual bool isRunning(void);
/*
* Updates the current line distance and switches the state, if an upper bound or the minimum line distance has been reached:
* ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED
* @param[in] line_detected: true if line detected (line_distance is valid), false if no line detected
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry
* @return angular velocity (updated value, taken the current line_distance into account)
*/
virtual double update(bool line_detected, double line_distance, double line_width, double yaw_angle);
/*
* Returns the lowest line dictance found by current search (if isRunning()==true) or by last search (if isRunning()==false)
*/
virtual double getBestLineDistance(void) { return m_line_distance_best; }
/*
* Returns the lowest line width found by current search (if isRunning()==true) or by last search (if isRunning()==false)
*/
virtual double getBestLineWidth(void) { return m_line_width_best; }
/*
* Returns the yaw angle at lowest line dictance resp. line width found by current search (if isRunning()==true) or by last search (if isRunning()==false)
*/
virtual double getBestYawAngle(void) { return m_yaw_angle_best; }
/*
* Returns the difference of two angles in range -PI and +PI
* @param[in] angle1: first angle in range -PI and +PI
* @param[in] angle2: second angle in range -PI and +PI
* @return (angle2 - angle1) in range -PI and +PI
*/
static inline double deltaAngle(double angle1, double angle2)
{
double delta = angle2 - angle1;
while (delta < -M_PI) delta += 2 * M_PI;
while (delta > +M_PI) delta -= 2 * M_PI;
return delta;
}
/*
* Returns the sign of a value, i.e. a shortcut for
* +1 if value >= epsilon,
* -1 if value <= -epsilon, or
* 0 otherwise
*/
static inline int signum(double value, double epsilon)
{
if(value >= epsilon)
return 1;
else if(value <= -epsilon)
return -1;
return 0;
}
protected:
/*
* ADJUST_HEADING_STATE_ENUM enumerates the state machine:
* ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED
*/
typedef enum ADJUST_HEADING_STATE_ENUM
{
ADJUST_HEADING_STATE_STOPPED, // initial state
ADJUST_HEADING_STATE_SEARCH_FORWARD_1, // currently searching by rotating forward until upper bound reached (1. path after start)
ADJUST_HEADING_STATE_SEARCH_BACKWARD_2, // currently searching by rotating backward until upper bound or minimum reached (2. path after forward search)
ADJUST_HEADING_STATE_SEARCH_FORWARD_3, // currently searching by rotating forward until minimum reached (final path)
ADJUST_HEADING_MAX_STATES // number of states
} ADJUST_HEADING_STATE;
/*
* Returns true, if line distance or line width are above their upper bounds. In this case, search direction is inverted. Otherwise false is returned.
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry (currently unused)
* @return true, if measurement (line distance or line width) out of configured bounds
*/
bool greaterThanUpperBounds(double line_distance, double line_width, double yaw_angle);
/*
* Computes the feature value from a new measurement.
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry (currently unused)
* @return feature value
*/
double computeFeature(double line_distance, double line_width, double yaw_angle);
/*
* member data
*/
AdjustHeadingConfig m_adjust_heading_cfg; // Parameter (upper and lower bounds) configuring the search
ADJUST_HEADING_STATE m_adjust_heading_state; // simple state machine: ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED
bool m_use_line_width; // minimize line distance (false,default) or line width (true)
double m_line_distance_at_start; // line distance at start of search
double m_line_width_at_start; // line width at start of search
double m_yaw_angle_at_start; // yaw angle at start of search
double m_line_distance_best; // lowest line distance found while searching
double m_line_width_best; // lowest line width found while searching
double m_feature_value_best; // lowest feature value found while searching
double m_yaw_angle_best; // yaw angle at lowest line distance
double m_final_step_delta_heading; // delta angle at final step (:= <current yaw angle> - m_yaw_angle_best)
int m_final_step_toggle_cnt; // counts possible toggling around the minimum line distance at the final optimization step
AngularZCtr m_wait_at_state_transition; // at state transitions: increase/decrease angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity
ros::Time m_states_start_time[ADJUST_HEADING_MAX_STATES]; // test only: limit time amount in seconds to stay in current search state; after <max_state_duration> seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions
/*
* class AdjustHeadingAutoPrinter: Utility to print values of class AdjustHeading automatically in destructor.
*/
class AdjustHeadingAutoPrinter
{
public:
AdjustHeadingAutoPrinter(AdjustHeading* adjuster = 0, bool line_detected = 0, double line_distance = 0, double line_width = 0, double cur_dist = 0, double cur_yaw = 0, double* p_angular_z = 0)
: m_adjuster(adjuster), m_line_detected(line_detected), m_line_distance(line_distance), m_line_width(line_width), m_cur_dist(cur_dist), m_cur_yaw(cur_yaw),
m_angular_z(p_angular_z), m_prev_state(adjuster?(adjuster->m_adjust_heading_state):ADJUST_HEADING_STATE_STOPPED) {}
virtual ~AdjustHeadingAutoPrinter()
{
if(m_adjuster && (m_prev_state != ADJUST_HEADING_STATE_STOPPED || m_adjuster->m_adjust_heading_state != ADJUST_HEADING_STATE_STOPPED))
{
std::string timestamp = sick_line_guidance_demo::TimeFormat::formatDateTime();
ROS_INFO("%sADJUST HEADING: detected=%d, linedist=%.3lf, linewidth=%.3lf, startdist=%.3lf, startwidth=%.3lf, value=%.6lf, best=%.6lf, curyaw=%.4lf*PI, bestyaw=%.4lf*PI, angular.z=%.2lf*PI, WAITSTATE=%d, PREVSTATE=%d, NEXTSTATE=%d",
timestamp.c_str(), (int)m_line_detected, m_line_distance, m_line_width, m_adjuster->m_line_distance_at_start, m_adjuster->m_line_width_at_start, m_cur_dist, m_adjuster->m_feature_value_best,
m_cur_yaw/M_PI, m_adjuster->m_yaw_angle_best/M_PI, *m_angular_z/M_PI, (int)m_adjuster->m_wait_at_state_transition.isRunning(), (int)m_prev_state, (int)m_adjuster->m_adjust_heading_state);
}
}
protected:
AdjustHeading* m_adjuster;
ADJUST_HEADING_STATE m_prev_state; // previous state (m_adjuster->m_adjust_heading_state at constructor)
bool m_line_detected; // true: sensor detected line, line_distance and line_width valid
double m_line_distance; // current sensor line distance (line center point)
double m_line_width; // current sensor line width
double m_cur_dist; // current distance (line distance or line_width)
double m_cur_yaw; // current heading (yaw angle)
double* m_angular_z; // heading returned by AdjustHeading
};
}; // class AdjustHeading
} // namespace sick_line_guidance_demo
#endif // __ROBOT_FSM_ADJUST_HEADING_H_INCLUDED

149
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/barcodes.h Executable file
View File

@@ -0,0 +1,149 @@
/*
* barcodes implements a container for barcodes for sick_line_guidance_demo (position and label).
*
* 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_DEMO_BARCODES_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_BARCODES_H_INCLUDED
namespace sick_line_guidance_demo
{
/*
* class Barcode implements a container for barcode properties for sick_line_guidance_demo (position and label)..
*/
class Barcode
{
public:
/*
* Constructor
*/
Barcode() : m_label(""), m_outer_rect_world(0,0,0,0), m_inner_rect_world(0,0,0,0), m_outer_rect_map(0,0,0,0), m_inner_rect_map(0,0,0,0), m_flipped(false){}
/*
* Get/set barcode label, f.e. "0101"
*/
std::string & label(void) { return m_label; }
/*
* Retuns the barcode label as number f.e. 101
*/
size_t labelCode(void) { return m_label.empty() ? 0 : ((size_t)std::atol(m_label.c_str())); }
/*
* Get/set barcode outer rect in world coordinates [meter]
*/
cv::Rect2d & outerRectWorld(void) { return m_outer_rect_world; }
/*
* Get/set barcode inner rect (label area) in world coordinates [meter]
*/
cv::Rect2d & innerRectWorld(void) { return m_inner_rect_world; }
/*
* Get/set barcode rect in image map coordinates [pixel]
*/
cv::Rect & outerRectMap(void) { return m_outer_rect_map; }
/*
* Get/set barcode rect in image map coordinates [pixel]
*/
cv::Rect & innerRectMap(void) { return m_inner_rect_map; }
/*
* Get/set barcode flipped (true or false)
*/
bool & flipped(void) { return m_flipped; }
/*
* returns the barcode center (center of the outer rect) in world coordinates [meter]
*/
cv::Point2d centerWorld(void) { return cv::Point2d(m_outer_rect_world.x + m_outer_rect_world.width/2.0, m_outer_rect_world.y + m_outer_rect_world.height/2.0); }
/*
* returns the barcode center (center of the outer rect) in map coordinates [pixel]
*/
cv::Point2d centerMap(void) { return cv::Point2d(m_outer_rect_map.x + m_outer_rect_map.width/2.0, m_outer_rect_map.y + m_outer_rect_map.height/2.0); }
protected:
/*
* member data
*/
std::string m_label; // barcode label, f.e. "0101"
cv::Rect2d m_outer_rect_world; // barcode outer rect in world coordinates [meter]
cv::Rect2d m_inner_rect_world; // barcode inner rect (label area) in world coordinates [meter]
cv::Rect m_outer_rect_map; // barcode outer rect in image map coordinates [pixel]
cv::Rect m_inner_rect_map; // barcode inner rect (label area) in image map coordinates [pixel]
bool m_flipped; // barcode flipped (true or false)
}; // class Barcode
/*
* class BarcodeUtil implements support tools for Barcodes (configuration from xml-file, etc.)
*/
class BarcodeUtil
{
public:
/*
* reads a list of barcodes from xml-file
* @param[in] barcode_xml_file xml-file, f.e. "demo_barcodes.xml"
* @return list of barcodes (empty list, if xmlfile could not be read)
*/
static std::vector<Barcode> readBarcodeXmlfile(const std::string & barcode_xml_file);
};
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_BARCODES_H_INCLUDED

124
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/explore_line_state.h Executable file
View File

@@ -0,0 +1,124 @@
/*
* ExploreLineState implements the state to explore a line for sick_line_guidance_demo.
* As long as ols does not detect a line, cmd_vel messages are published to search a line.
* Currently, the TurtleBot just moves straight forwared until a line is detected.
* Input: ols messages
* Output: cmd_vel 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_DEMO_EXPLORE_LINE_STATE_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_EXPLORE_LINE_STATE_H_INCLUDED
#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include "sick_line_guidance/OLS_Measurement.h"
#include "sick_line_guidance_demo/robot_fsm_context.h"
namespace sick_line_guidance_demo
{
/*
* ExploreLineState implements the state to explore a line for sick_line_guidance_demo.
* As long as ols does not detect a line, cmd_vel messages are published to search a line.
* Currently, the TurtleBot just moves straight forwared until a line is detected.
* Input: ols messages
* Output: cmd_vel messages
*/
class ExploreLineState
{
public:
/*
* Constructor
* @param[in] nh ros handle
* @param[in] context shared fsm context
*/
ExploreLineState(ros::NodeHandle* nh=0, RobotFSMContext* context = 0);
/*
* Destructor
*/
~ExploreLineState();
/*
* Clears all internal states
*/
void clear(void);
/*
* Runs the explore line state until line is detected (or a fatal error occures).
* @return FOLLOW_LINE in case of line detected, or EXIT in case ros::ok()==false.
*/
RobotFSMContext::RobotState run(void);
protected:
/*
* member data
*/
RobotFSMContext* m_fsm_context; // shared state context
/*
* Configuration
*/
double m_exploreSpeed; // default linear velocity to explore a line
ros::Rate m_explore_line_rate; // frequency to update explort line state, default: 20 Hz
double m_ols_message_timeout; // timeout for ols messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
double m_odom_message_timeout; // timeout for odom messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
}; // class ExploreLineState
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_EXPLORE_LINE_STATE_H_INCLUDED

115
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/figure_eight_fsm.h Executable file
View File

@@ -0,0 +1,115 @@
/*
* FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in a figure of eight.
*
* 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_DEMO_FIGURE_EIGHT_FSM_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_FIGURE_EIGHT_FSM_H_INCLUDED
#include <nav_msgs/Odometry.h>
namespace sick_line_guidance_demo
{
/*
* class FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in a figure of eight.
*/
class FigureEightVelocityFSM
{
public:
/*
* Constructor
*/
FigureEightVelocityFSM();
/*
* Destructor
*/
~FigureEightVelocityFSM();
/*
* Next cycle, internal state is updated, velocity message may switch to next movement
*/
void update(void);
/*
* Returns the cmd_vel message for the current movement
*/
geometry_msgs::Twist getVelocity(void);
protected:
/*
* VelocityState := cmd_vel message and its duration
* class VelocityState is just a container for a cmd_vel message and its duration
*/
class VelocityState
{
public:
geometry_msgs::Twist cmd_vel;
ros::Duration duration;
};
/*
* member data
*/
int m_state_cnt;
ros::Time m_next_state_switch;
std::vector<VelocityState> m_vec_vel_states;
}; // class FigureEightVelocityFSM
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_FIGURE_EIGHT_FSM_H_INCLUDED

137
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/follow_line_state.h Executable file
View File

@@ -0,0 +1,137 @@
/*
* FollowLineState implements the state to follow a line for sick_line_guidance_demo.
* As long as ols detects a line, cmd_vel messages are published to follow this line.
* Input: ols and odometry messages
* Output: cmd_vel 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_DEMO_FOLLOW_LINE_STATE_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_FOLLOW_LINE_STATE_H_INCLUDED
#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include "sick_line_guidance/OLS_Measurement.h"
#include "sick_line_guidance_demo/robot_fsm_context.h"
namespace sick_line_guidance_demo
{
/*
* class FollowLineState implements the state to follow a line for sick_line_guidance_demo.
* As long as ols detects a line, cmd_vel messages are published to follow this line.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
class FollowLineState
{
public:
/*
* Constructor
* @param[in] nh ros handle
* @param[in] context shared fsm context
*/
FollowLineState(ros::NodeHandle* nh=0, RobotFSMContext* context = 0);
/*
* Destructor
*/
~FollowLineState();
/*
* Clears all internal states (pid etc.)
*/
void clear(void);
/*
* Runs the follow line state until line is lost (or a fatal error occures).
* @return EXPLORE_LINE in case of line lost, or EXIT in case ros::ok()==false.
*/
RobotFSMContext::RobotState run(void);
protected:
/*
* member data
*/
RobotFSMContext* m_fsm_context; // shared state context
ros::Time m_last_barcode_detected_time; // timestamp of last detected barcode
/*
* Configuration
*/
double m_pid_kp; // P parameter of PID control
double m_pid_ki; // I parameter of PID control
double m_pid_kd; // D parameter of PID control
double m_pid_setpoint; // setpoint parameter of PID control
double m_followSpeed; // default linear velocity to follow a line
ros::Rate m_followLineRate; // frequency to update follow line state, default: 20 Hz
double m_noLineTime ; // time in seconds before switching to state explore line because of lost line
double m_sensorLineWidth; // measured line width at 0 degree, 29 mm for an OLS mounted 65 mm over ground, 20 mm for an OLS mounted 100 mm over ground
double m_sensorLineMeasurementJitter; // tolerate some line measurement jitter when adjusting the heading
double m_adjustHeadingAngularZ; // velocity.angular.z to adjust robots heading
double m_adjustHeadingLcpDeviationThresh; // start to adjust heading, if the line distance increases over time (deviation of 1D-regression of line center points is above threshold lcpDeviationThresh)
double m_adjustHeadingLcpThresh; // start to adjust heading, if the line distance in meter (abs value) is above this threshold
double m_adjustHeadingDeltaAngleEpsilon; // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon
double m_adjustHeadingMinDistanceToLastAdjust; // move at least some cm before doing next heading adjustment
double m_olsMessageTimeout; // timeout for ols messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
double m_odomMessageTimeout; // timeout for odom messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
double m_seconds_at_barcode; // time in seconds to wait at barcode
int m_ols_simu; // ols simulation (default: 0), test only
}; // class FollowLineState
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_FOLLOW_LINE_STATE_H_INCLUDED

241
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/image_util.h Executable file
View File

@@ -0,0 +1,241 @@
/*
* ImageUtil implements utility functions for images and maps.
*
* 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_DEMO_IMAGE_UTIL_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_IMAGE_UTIL_H_INCLUDED
#include <opencv2/core.hpp>
namespace sick_line_guidance_demo
{
/*
* class LineDetectionResult is a container for a line center point of a detected line, its width and its distance to the robot.
*/
class LineDetectionResult
{
public:
/*
* LineDetectionResult constructor
*/
LineDetectionResult(const cv::Point2d & center_pos = cv::Point2d(0,0), const cv::Point2d & start_pos = cv::Point2d(0,0), const cv::Point2d & end_pos = cv::Point2d(0,0), double line_width = 0, double center_dist = 0)
: m_center_pos(center_pos), m_start_pos(start_pos), m_end_pos(end_pos), m_line_width(line_width), m_center_dist(center_dist)
{
}
/*
* get/set line center position (x,y)
*/
cv::Point2d & centerPos(void) { return m_center_pos; }
const cv::Point2d & centerPos(void) const { return m_center_pos; }
/*
* get/set line center position (x,y)
*/
cv::Point2d & startPos(void) { return m_start_pos; }
const cv::Point2d & startPos(void) const { return m_start_pos; }
/*
* get/set line center position (x,y)
*/
cv::Point2d & endPos(void) { return m_end_pos; }
const cv::Point2d & endPos(void) const { return m_end_pos; }
/*
* get/set line width
*/
double & lineWidth(void) { return m_line_width; }
const double & lineWidth(void) const { return m_line_width; }
/*
* get/set distance to center position
*/
double & centerDistance(void) { return m_center_dist; }
const double & centerDistance(void) const { return m_center_dist; }
protected:
cv::Point2d m_center_pos; // line center position (x,y)
cv::Point2d m_start_pos; // line start position (x,y)
cv::Point2d m_end_pos; // line end position (x,y)
double m_line_width; // line width
double m_center_dist; // distance to center position
};
/*
* class ImageUtil implements utility functions for images and maps.
*/
class ImageUtil
{
public:
/*
* returns the pixel of a map at positions (posx, posy), or a default value (white, BGR=255,255,255) if (posx, posy) is outside the image (red := out of map).
* @param[in] map_img image (navigation map)
* @param[in] posx x-position in image map coordinates [pixel]
* @param[in] posy y-position in image map coordinates [pixel]
* @return pixel (cv::Vec3b)
*/
static inline cv::Vec3b getMapPixel(const cv::Mat & map_img, int posx, int posy, const cv::Vec3b & default_pixel = cv::Vec3b(255,255,255))
{
if(posx >= 0 && posy >= 0 && posx < map_img.cols && posy < map_img.rows)
{
return map_img.at<cv::Vec3b>(posy,posx);
}
return default_pixel; // out of image map
}
/*
* returns true, if two pixel (type cv::Vec3b) have identical values, or false otherwise.
* shortcut for (a[0] == b[0] && a[1] == b[1] && a[2] == b[2])
* @param[in] a first pixel, to be compared with b
* @param[in] b seoncd pixel, to be compared with a
* @return true, if a[n] == b[n] for all n, otherwise false.
*/
static inline bool cmpPixel(const cv::Vec3b & a, const cv::Vec3b & b)
{
return (a[0] == b[0] && a[1] == b[1] && a[2] == b[2]);
}
/*
* returns true, if the pixel at position (x,y) is on a line (i.e. a black pixel), or false otherwise.
* @param[in] map_img image (navigation map)
* @param[in] posx x-position in image map coordinates [pixel]
* @param[in] posy y-position in image map coordinates [pixel]
* @return true, if map at (x,y) is a black line pixel, or otherwise false.
*/
static inline bool isLinePixel(const cv::Mat & map_img, int posx, int posy)
{
return (cmpPixel(getMapPixel(map_img, posx, posy), cv::Vec3b(0,0,0)));
}
/*
* returns true, if the pixel at position pos is on a line (i.e. a black pixel), or false otherwise.
* @param[in] map_img image (navigation map)
* @param[in] pos (x,y)-position in image map coordinates [pixel]
* @return true, if map at (pos.x,pos.y) is a black line pixel, or otherwise false.
*/
static inline bool isLinePixel(const cv::Mat & map_img, const cv::Point & pos)
{
return isLinePixel(map_img, pos.x, pos.y);
}
/*
* computes and returns the position of a point with a distance <radius> and in direction <heading> from a given point <base_pos>.
* @param[in] base_pos start position in world coordinates [meter]
* @param[in] heading angle between given point <base_pos> and the returned point
* @param[in] radius distance of the returned point to <base_pos>
* @return position of a point with a distance <radius> and in direction <heading> from a given point <base_pos> in world coordinates [meter]
*/
static cv::Point2d getWorldPointInDirection(const cv::Point2d & base_pos, double heading, double radius);
/*
* computes and returns the position of a point with a distance <radius> and in direction <heading> from a given point <base_pos>.
* @param[in] base_pos start position in map coordinates [pixel]
* @param[in] heading angle between given point <base_pos> and the returned point
* @param[in] radius distance of the returned point to <base_pos>
* @return position of a point with a distance <radius> and in direction <heading> from a given point <base_pos> in map coordinates [pixel]
*/
static cv::Point getMapPointInDirection(const cv::Point & base_pos, double heading, double radius);
/*
* Detects and returns the line center points on a map, which can be seen
* by a robot at position <robot_pos> moving in directions <robot_heading>.
* Lines are detected +- 90 degree of <robot_heading>.
* @param[in] map_img image (navigation map)
* @param[in] robot_map_pos robots position on the navigation map
* @param[in] robot_heading robots moving direction (i.e. robots yaw angle)
* @return list of line detection results (center points etc.) in map coordinates [pixel]
*/
static std::vector<LineDetectionResult> detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading);
/*
* Detects and returns the line center points on a map, in front of
* a robot at position <robot_pos> moving in directions <robot_heading>.
* Lines are detected in heading of <robot_heading>.
* @param[in] map_img image (navigation map)
* @param[in] robot_map_pos robots position on the navigation map
* @param[in] robot_heading robots moving direction (i.e. robots yaw angle)
* @return list of line detection results (center points etc.) in map coordinates [pixel]
*/
static std::vector<sick_line_guidance_demo::LineDetectionResult> detectLineCenterInHeadingDirection(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading);
protected:
/*
* computes and returns the max possible euclidean distance of a robot position to the corner points of an image.
* @param[in] robot_pos robots position on the navigation map [pixel]
* @param[in] dimx width of navigation map [pixel]
* @param[in] dimy height of navigation map [pixel]
* @return max possible distance [pixel]
*/
static double computeMaxDistanceToCorner(const cv::Point & robot_pos, int dimx, int dimy);
/*
* detects and returns the line center points on a map, which can be seen
* by a robot at position <robot_map_pos> moving in directions <robot_heading>.
* @param[in] map_img image (navigation map)
* @param[in] robot_map_pos robots position on the navigation map
* @param[in] search_start_pos start point for line iteration
* @param[in] search_end_pos end point for line iteration
* @return list of line detection results (center points etc.) in map coordinates [pixel]
*/
static std::vector<sick_line_guidance_demo::LineDetectionResult> detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, const cv::Point2d & search_start_pos, const cv::Point2d & search_end_pos);
}; // class ImageUtil
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_IMAGE_UTIL_H_INCLUDED

249
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_mapper.h Executable file
View File

@@ -0,0 +1,249 @@
/*
* NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position,
* detect lines and barcodes in the map plus their distance to the robot,
* and transforms them invers into world coordinates.
*
* 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_DEMO_NAVIGATION_MAPPER_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_MAPPER_H_INCLUDED
#include <limits.h>
#include <string>
#include <opencv2/core.hpp>
#include <opencv2/videoio.hpp>
#include <nav_msgs/Odometry.h>
#include <gazebo_msgs/ModelStates.h>
#include "sick_line_guidance_demo/barcodes.h"
#include "sick_line_guidance_demo/image_util.h"
#include "sick_line_guidance_demo/ols_measurement_simulator.h"
#include "sick_line_guidance_demo/set_get.h"
namespace sick_line_guidance_demo
{
/*
* struct LineSensorConfig collects the line sensor configuration setting (sensor parameter and mounting settings):
* line_sensor_detection_width: default: 0.130, Width of the detection area of the line sensor (meter), 130 mm for sensor mounted 65 mm over ground
* line_sensor_scaling_dist: default: 180.0/133.0, Scaling between physical distance to the line center and measured line center point (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height
* line_sensor_scaling_width: default: 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_sensor_mounted_right_to_left: default: true, Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right
*/
typedef struct LineSensorConfigStruct
{
LineSensorConfigStruct() : line_sensor_detection_width(0.130), line_sensor_scaling_dist(180.0/133.0), line_sensor_scaling_width(29.0/20.0), line_sensor_mounted_right_to_left(true) {}
double line_sensor_detection_width; // default: 0.130, Width of the detection area of the line sensor (meter), 130 mm for sensor mounted 65 mm over ground
double line_sensor_scaling_dist; // default: 180.0 / 133.0, Scaling between physical distance to the line center and measured line center point (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height
double line_sensor_scaling_width; // default: 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)
bool line_sensor_mounted_right_to_left; // default: true, Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right
} LineSensorConfig;
/*
* class NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position,
* detect lines and barcodes in the map plus their distance to the robot,
* and transforms them invers into world coordinates.
*/
class NavigationMapper
{
public:
/*
* Constructor
* @param[in] map_imagefile navigation map, image file containing the map, f.e. "demo_map_02.png"
* @param[in] intrinsic_xmlfile xmlfile with intrinsic parameter to transform position from navigation map (pixel) to real world (meter) and vice versa, f.e. "cam_intrinsic.xml" with cx=cy=660, fx=fy=1
* @param[in] barcode_xmlfile xmlfile with a list of barcodes with label and position, f.e. "demo_barcodes.xml"
* @param[in] ros_topic_output_ols_messages ROS topic for simulated OLS_Measurement messages, "/ols" (activated) in simulation and "" (deactivated) in demo application
* @param[in] sensor_config line sensor configuration setting (sensor parameter and mounting settings)
* @param[in] visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled.
*/
NavigationMapper(ros::NodeHandle* nh=0, const std::string & map_imagefile = "", const std::string & intrinsic_xmlfile = "", const std::string & barcode_xmlfile = "", const std::string & ros_topic_output_ols_messages = "",
const LineSensorConfig & sensor_config = LineSensorConfig(), int visualize = 0);
/*
* Destructor
*/
~NavigationMapper();
/*
* message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom".
* It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot,
* and transform them invers into world coordinates.
* @param[in] msg odometry message (input)
*/
virtual void messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
/*
* message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols".
* It displays the OLS measurement (line info and barcodes), if visualization is enabled.
* @param[in] msg OLS measurement message (input)
*/
virtual void messageCallbackOlsMeasurement(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg);
/*
* starts the message loop to handle ols and odometry messages received.
* Message handling runs in background thread started by this function.
*/
virtual void start(void);
/*
* stops the message loop.
*/
virtual void stop(void);
protected:
/*
* transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel]
* @param[in] world_pos xy-position in world coordinates [meter]
* @param[out] map_pos xy-position in image map coordinates [pixel]
*/
virtual void transformPositionWorldToMap(const cv::Point2d & world_pos, cv::Point2d & map_pos);
/*
* transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel]
* @param[in] world_pos xy-position in world coordinates [meter]
* @return xy-position in image map coordinates [pixel]
*/
virtual cv::Point transformPositionWorldToMap(const cv::Point2d & world_pos);
/*
* transforms a xy-position in image map coordinates [pixel] to a xy-position in world coordinates [meter]
* @param[in] map_pos xy-position in image map coordinates [pixel]
* @return xy-position in world coordinates [meter]
*/
virtual cv::Point2d transformPositionMapToWorld(const cv::Point2d & map_pos);
/*
* transforms a rectangle in world coordinates [meter] into a rectangle in image map coordinates [pixel]
* @param[in] world_rect rectangle in world coordinates [meter]
* @return rectangle in image map coordinates [pixel]
*/
virtual cv::Rect transformRectWorldToMap(const cv::Rect2d & world_rect);
/*
* transforms an ols state from world coordinates [meter] to sensor units [meter], i.e. scales line distance (lcp) and line width
* from physical distances in the world to units of a sensor measurement; reverts function unscaleMeasurementToWorld():
* line distances (lcp) are scaled by line_sensor_scaling_dist: default: 180.0/133.0, Scaling between physical distance to the line center and measured line center point
* (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height
* line width are scaled by line_sensor_scaling_width: default: 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)
* @param[in] ols_state ols state in world coordinates [meter]
* @return ols measurement in sensor units [meter]
*/
virtual sick_line_guidance::OLS_Measurement scaleWorldToMeasurement(const sick_line_guidance::OLS_Measurement & ols_state);
/*
* transforms an ols measurement from sensor units [meter] to world coordinates [meter], i.e. scales line distance (lcp) and line width
* from sensor units to physical distances; reverts function scaleWorldToMeasurement().
* line distances (lcp) are scaled by 1 / line_sensor_scaling_dist
* line width are scaled by 1 / line_sensor_scaling_width
* @param[in] ols measurement in sensor units [meter]
* @return ols_state ols state in world coordinates [meter]
*/
virtual sick_line_guidance::OLS_Measurement unscaleMeasurementToWorld(const sick_line_guidance::OLS_Measurement & ols_message);
/*
* Runs the message loop to handle odometry and ols messages.
* It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot,
* and transform them invers into world coordinates.
* @param[in] msg odometry message (input)
*/
void messageLoop(void);
/*
* member data for navigation and mapping
*/
ros::Rate m_message_rate; // frequency to handle ols and odom messages, default: 20 Hz
cv::Mat m_map_img; // world map from image file
cv::Mat m_intrinsics; // intrinsic parameter to transforms the robots xy-positions from world [meter] into xy-positions in the image map [pixel]
cv::Mat m_intrinsics_inv; // inverted intrinsic parameter to transforms xy-positions in the image map [pixel] into robots xy-positions in the world world [meter]
std::vector<Barcode> m_barcodes; // list of barcodes
LineSensorConfig m_sensor_config; // sensor configuration
OLS_Measurement_Simulator m_ols_measurement_simulator; // Simulator for OLS measurement messages, prediction of expected OLS measurement messages
sick_line_guidance_demo::SetGet<sick_line_guidance::OLS_Measurement> m_ols_measurement_sensor; // OLS measurement message received from topic "/ols" (measurement in sensor units)
sick_line_guidance_demo::SetGet<sick_line_guidance::OLS_Measurement> m_ols_measurement_world; // OLS measurement message received from topic "/ols" (measurement scaled to world coordinates)
sick_line_guidance_demo::SetGet<nav_msgs::Odometry> m_odom_msg; // odometry message received from topic "/odom"
bool m_message_thread_run; // flag to start and stop m_message_thread
boost::thread* m_message_thread; // thread to run message loop
typedef enum NAVIGATION_STATE_ENUM
{
INITIAL, // navigation: initial state
SEARCH_LINE, // navigation: search for a line (initially or whenever line lost)
FOLLOW_LINE // navigation: follow a line
} NAVIGATION_STATE;
NAVIGATION_STATE m_navigation_state; // enumerates the navigational state: search for or follow a line
/*
* member data for error simulation and testing
*/
double m_error_simulation_burst_no_line_duration; // error simulation: duration of "no line detected" bursts in seconds, default: 0.0 (disabled)
double m_error_simulation_burst_no_line_frequency; // error simulation: frequency of "no line detected" bursts in 1/seconds, default: 0.0 (disabled)
ros::Time m_error_simulation_no_line_start;
ros::Time m_error_simulation_no_line_end;
/*
* member data for debugging and visualization
*/
int m_visualize; // visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled.
cv::Mat m_window_img; // image to plot and visualize, if m_visualize is true
std::string m_window_name; // named window for visualization, if m_visualize is true
cv::VideoWriter m_video_write; // output video writer
}; // class NavigationMapper
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_MAPPER_H_INCLUDED

234
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/navigation_util.h Executable file
View File

@@ -0,0 +1,234 @@
/*
* NavigationUtil implements utility functions for 2D/3D-transforms and navigation.
*
* 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_DEMO_NAVIGATION_UTIL_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_UTIL_H_INCLUDED
#include <opencv2/core.hpp>
#include <nav_msgs/Odometry.h>
#include <gazebo_msgs/ModelStates.h>
#include "sick_line_guidance/OLS_Measurement.h"
#include "sick_line_guidance_demo/image_util.h"
namespace sick_line_guidance_demo
{
/*
* class NavigationUtil implements utility functions for 2D/3D-transforms and navigation.
*/
class NavigationUtil
{
public:
/*
* Returns the difference of two angles in range -PI and +PI
* @param[in] angle1: first angle in range -PI and +PI
* @param[in] angle2: second angle in range -PI and +PI
* @return (angle2 - angle1) in range -PI and +PI
*/
static inline double deltaAngle(double angle1, double angle2)
{
return unwrapAngle(angle2 - angle1);
}
/*
* Unwraps an angle to range -PI and +PI
* @return angle in range -PI and +PI
*/
static inline double unwrapAngle(double angle)
{
while (angle < -M_PI) angle += 2 * M_PI;
while (angle > +M_PI) angle -= 2 * M_PI;
return angle;
}
/*
* computes and return the euclidean distance between two points,
* shortcut for sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y))
* @param[in] a first point
* @param[in] b second point
* @return euclidean distance between a and b
*/
static inline double euclideanDistance(const cv::Point2d & a, const cv::Point2d & b)
{
double dx = a.x - b.x;
double dy = a.y - b.y;
return sqrt(dx * dx + dy * dy);
}
/*
* computes and return the euclidean distance between between a robot position and a line center point,
* i.e. a negative value if the line_center is left from robots position when moving in robot_heading, and
* a positive value if the line_center is right from robots position when moving in robot_heading
* @param[in] robot_pos first point (robots position)
* @param[in] line_center_pos second point (line center position)
* @param[in] robot_heading robots moving direction (i.e. robots yaw angle)
* @param[in] sensor_mounted_right_to_left Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right
* @return euclidean distance between a and b
*/
static double euclideanDistanceOrientated(const cv::Point2d & robot_pos, const cv::Point2d & line_center_pos, double robot_heading, bool sensor_mounted_right_to_left);
/*
* transforms and returns (x,y) position and yaw angle of an odometry message into a readable string.
* @param[in] msg odometry message (input)
* @return (x,y) position and yaw angle of odometry message as a readable string (output)
*/
static std::string toInfo(const nav_msgs::Odometry::ConstPtr& msg);
/*
* transforms and returns (x,y) position and yaw angle of an odometry message into a readable string.
* @param[in] msg odometry message (input)
* @return (x,y) position and yaw angle of odometry message as a readable string (output)
*/
static std::string toInfo(const nav_msgs::Odometry & msg);
/*
* transforms and returns a gazebo ModelStates message into a readable string.
* @param[in] msg gazebo ModelStates message (input)
* @param[in] start_idx index to start iteration of names and poses (input, default = 0: print all names and poses)
* @param[in] last_idx index to end iteration of names and poses (input, default = MAX_INT: print all names and poses)
* @return gazebo ModelStates message as a readable string (output)
*/
static std::string toInfo(const gazebo_msgs::ModelStates::ConstPtr& msg, size_t start_idx = 0, size_t last_idx = INT_MAX);
/*
* gets the robots xy-position and yaw angle from an odometry message
* @param[in] msg odometry message (input)
* @param[out] world_posx x-position in world coordinates [meter]
* @param[out] world_posy y-position in world coordinates [meter]
* @param[out] yaw_angle orientation (z-axis rotation) in rad
*/
static void toWorldPosition(const nav_msgs::Odometry & msg, double & world_posx, double & world_posy, double & yaw_angle);
/*
* gets the robots xy-position and yaw angle from an odometry message
* @param[in] msg odometry message (input)
* @param[out] world_posx x-position in world coordinates [meter]
* @param[out] world_posy y-position in world coordinates [meter]
* @param[out] yaw_angle orientation (z-axis rotation) in rad
*/
static void toWorldPosition(const nav_msgs::Odometry::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle);
/*
* gets the robots xy-position and yaw angle from a gazebo ModelStates message
* @param[in] msg gazebo ModelStates message (input)
* @param[out] world_posx x-position in world coordinates [meter]
* @param[out] world_posy y-position in world coordinates [meter]
* @param[out] yaw_angle orientation (z-axis rotation) in rad
*/
static void toWorldPosition(const gazebo_msgs::ModelStates::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle);
/*
* transforms an orientation from Quaternion to angles: roll (x-axis rotation), pitch (y-axis rotation), yaw (z-axis rotation).
* @param[in] quat_msg gazebo orientation (quaternion, input)
* @param[out] roll angle (x-axis rotation, output)
* @param[out] pitch angle (y-axis rotation, output)
* @param[out] yaw angle (z-axis rotation, output)
*/
static void transformOrientationToRollPitchYaw(const geometry_msgs::Quaternion & quat_msg, double & roll, double & pitch, double & yaw);
/*
* returns +1, if a point is on the right side of the robot, and -1 otherwise.
* see https://math.stackexchange.com/questions/274712/calculate-on-which-side-of-a-straight-line-is-a-given-point-located
* for the math under the hood.
* @param[in] xpoint x-position of the point
* @param[in] ypoint y-position of the point
* @param[in] xrobot x-position of the robot
* @param[in] yrobot y-position of the robot
* @param[in] heading heading of the robot
* @return +1 (point is on the right side) or -1 (point is on the left side)
*/
static int isPointRightFromRobot(double xpoint, double ypoint, double xrobot, double yrobot, double heading);
/*
* Selects line points within the sensor detection zone from a list of possible line center points
* @param[in] world_line_points list of possible line center points in world coordinates [meter]
* @param[in] robot_world_pos robot position in world coordinates [meter]
* @param[in] line_sensor_detection_width width of sensor detection zone (f.e. 0.180 for an OLS mounted in 0.1m height over ground, 0.130 for the demo system with OLS mounted in 0.065m height)
* @param[in] max_line_cnt max. number of lines (OLS: max. 3 lines)
* @return list of line points within the sensor detection zone
*/
static std::vector<LineDetectionResult> selectLinePointsWithinDetectionZone(std::vector<LineDetectionResult> & world_line_points, const cv::Point2d & robot_world_pos, double line_sensor_detection_width, size_t max_line_cnt = 3);
/*
* returns true, if a line is detected by ols measurement (bit 0, 1 or 2 of status is set), or false otherwise.
*/
static bool lineDetected(const sick_line_guidance::OLS_Measurement & ols_msg)
{
return ((ols_msg.status) & 0x7) != 0; // bit 0, 1 or 2 of status is set -> line detected
}
/*
* returns true, if a barcode is detected by ols measurement (bit 7 of status is set), or false otherwise.
*/
static bool barcodeDetected(const sick_line_guidance::OLS_Measurement & ols_msg)
{
return (((ols_msg.status) & 0x80) != 0); // bit 7 of status is set -> barcode valid
}
/*
* returns the barcode, if a barcode is detected by ols measurement, or 0 otherwise.
*/
static uint32_t barcode(const sick_line_guidance::OLS_Measurement & ols_msg)
{
if(barcodeDetected(ols_msg))
return ols_msg.barcode < 255 ? ols_msg.barcode : ols_msg.extended_code;
return 0;
}
}; // class NavigationUtil
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_NAVIGATION_UTIL_H_INCLUDED

171
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/ols_measurement_simulator.h Executable file
View File

@@ -0,0 +1,171 @@
/*
* ols_measurement_simulator simulates OLS_Measurement messages for sick_line_guidance_demo.
*
* OLS_Measurement_Simulator converts the distance between the simulated robot and
* the optical lines from the navigation map into OLS_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_LINE_GUIDANCE_DEMO_OLS_MEASUREMENT_SIMULATOR_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_OLS_MEASUREMENT_SIMULATOR_H_INCLUDED
#include <boost/thread.hpp>
#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/OLS_Measurement.h"
namespace sick_line_guidance_demo
{
/*
* class OLS_Measurement_Simulator converts the distance between the simulated robot and
* the optical lines from the navigation map into OLS_Measurement messages.
*/
class OLS_Measurement_Simulator
{
public:
/*
* Constructor
* @param[in] ros_topic_ols_messages ros topic for publishing OLS messages (empty: deactivated)
* @param[in] publish_rate rate to publish OLS measurements (if activated, default: 100)
*/
OLS_Measurement_Simulator(ros::NodeHandle* nh=0, const std::string & ros_topic_ols_messages = "", double publish_rate = 100);
/*
* Destructor
*/
virtual ~OLS_Measurement_Simulator();
/*
* Get the current OLS state
*/
virtual sick_line_guidance::OLS_Measurement GetState(void)
{
boost::lock_guard<boost::mutex> state_lockguard(m_ols_state_mutex);
return m_ols_state;
}
/*
* Set the current OLS state
*/
virtual void SetState(const sick_line_guidance::OLS_Measurement & ols_state)
{
boost::lock_guard<boost::mutex> state_lockguard(m_ols_state_mutex);
m_ols_state = ols_state;
}
/*
* Returns the ros topic for publishing ols messages, configured in the constructor.
* If not running a simulation (the default), this topic is empty (no ols messages published)
*/
std::string getPublishTopic(void){ return m_ros_topic_ols_messages;}
/*
* publish the current OLS state
*/
void publish(void);
/*
* publish the current OLS state in background task (publish with fixed rate)
*/
virtual void schedulePublish(void);
/*
* Initializes ols_state for one line (position, width and status)
*/
static void setLine(sick_line_guidance::OLS_Measurement & ols_state, float position, float width);
/*
* Initializes ols_state for detected lines (position, width and status for 0, 1, 2 or 3 lines)
*/
static void setLines(sick_line_guidance::OLS_Measurement & ols_state, std::vector<LineDetectionResult> & line_points);
/*
* Sets the barcode of an ols_state
*/
static void setBarcode(sick_line_guidance::OLS_Measurement & ols_state, size_t label, bool flipped);
/*
* rounds a double value to a given float precision, f.e. roundPrecision(lcp, 0.001) to round a line center point to millimeter precision.
*/
static float roundPrecision(double value, double precision);
protected:
/*
* thread callback, just publishes the current OLS state with a const rate
*/
void runPublishThread(void);
/*
* member data
*/
std::string m_ros_topic_ols_messages; // ros topic for publishing OLS messages (empty: deactivated)
ros::Publisher m_ols_publisher; // publisher for ols measurement messages on topic "/ols"
boost::thread* m_ols_publish_thread; // thread to publish ols measurement messages
bool m_publish_scheduled; // if true, m_ols_publish_thread will publish m_ols_state
ros::Rate m_ols_publish_rate; // rate to publish the current OLS state (default: 100)
sick_line_guidance::OLS_Measurement m_ols_state; // the current OLS state
boost::mutex m_ols_state_mutex; // lock guard for access to m_ols_state
}; // class OLS_Measurement_Simulator
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_OLS_MEASUREMENT_SIMULATOR_H_INCLUDED

32
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/pid.h Executable file
View File

@@ -0,0 +1,32 @@
#ifndef PID_H
#define PID_H
// using namespace std;
class PID_Controller
{
public:
PID_Controller(double dt, double kp, double ki, double kd); //Contstructor
double calculate_output(double setpoint_value, double actual_value);
void setParams(double dt, double kp, double ki, double kd);
void getParams(double& dt, double& kp, double& ki, double& kd);
void reset();
void clear_state();
~PID_Controller(); //Destructor
private:
double _kp; // Proportional Factor
double _ki; // Integrate Factor
double _kd; // Derivation Factor
double _dt; // Sample Time
double _e = 0; // controller deviation
double _e_int = 0; // controller integrator
double _e_diff = 0; // controller derivator
double _e_z = 0; // e * z⁻1
double _yp = 0; // P-Controller Output
double _yi = 0; // I-Controller Output
double _yd = 0; // D-Controller Output
};
#endif

176
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/regression_1d.h Executable file
View File

@@ -0,0 +1,176 @@
/*
* regression_1d estimates a one-dimensional regresssion (value over time).
*
* 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_DEMO_REGRESSION_1D_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_REGRESSION_1D_H_INCLUDED
namespace sick_line_guidance_demo
{
/*
* class Regression1D implements a one-dimensional regression over a fixed number of data points.
*/
class Regression1D
{
public:
/*
* Constructor
* @param[in] n: number of data points
*/
Regression1D(int n = 0) : m_solved(false), m_data_cnt(0)
{
if(n > 0)
{
m_left_mat = cv::Mat(n, 2, CV_64F);
m_right_mat = cv::Mat(n, 1, CV_64F);
m_solved_mat = cv::Mat(2, 1, CV_64F);
clear();
}
}
/*
* Updates a value over time and performs a new regression.
*/
inline void update(double value, double time = ros::Time::now().toSec())
{
if(!m_left_mat.empty() && !m_right_mat.empty() && !m_solved_mat.empty())
{
assert(m_left_mat.cols == 2);
assert(m_right_mat.cols == 1);
assert(m_solved_mat.cols == 1);
assert(m_solved_mat.rows == 2);
assert(m_left_mat.rows == m_right_mat.rows);
// Move all rows one up
for(size_t y = 1; y < m_left_mat.rows; y++)
m_left_mat.at<double>(y - 1, 0) = m_left_mat.at<double>(y,0);
for(size_t y = 1; y < m_right_mat.rows; y++)
m_right_mat.at<double>(y - 1, 0) = m_right_mat.at<double>(y,0);
// Copy the new data to the last row
m_left_mat.at<double>(m_left_mat.rows - 1, 0) = time;
m_right_mat.at<double>(m_right_mat.rows - 1, 0) = value;
// Compute regression by solving m_left_mat * m_solved_mat = m_right_mat,
// where m_solved_mat contains the regression coefficients
m_data_cnt++;
if(m_data_cnt >= m_left_mat.rows && m_data_cnt >= m_right_mat.rows)
{
cv::Mat left_work;
cv::Mat right_work;
m_left_mat.copyTo(left_work);
m_right_mat.copyTo(right_work);
m_solved = cv::solve(left_work, right_work, m_solved_mat, cv::DECOMP_SVD);
}
}
}
/*
* Returns offset and deviation of the regression line.
*/
inline bool getRegression(double & offset, double & deviation)
{
if(m_solved && !m_solved_mat.empty())
{
assert(m_solved_mat.cols == 1);
assert(m_solved_mat.rows == 2);
offset = m_solved_mat.at<double>(1,0);
deviation = m_solved_mat.at<double>(0,0);
}
else
{
offset = 0;
deviation = 0;
}
return m_solved;
}
/*
* Returns the deviation part of the regression.
*/
inline double getDeviation(void)
{
return (m_solved && !m_solved_mat.empty()) ? (m_solved_mat.at<double>(0,0)) : 0;
}
/*
* Clears all values.
*/
inline void clear(void)
{
m_solved = false;
m_data_cnt = 0;
m_left_mat = 0;
m_right_mat = 0;
m_solved_mat = 0;
for(size_t y = 0; y < m_left_mat.rows; y++)
{
m_left_mat.at<double>(y,1) = 1;
}
}
protected:
/*
* member data
*/
bool m_solved;
size_t m_data_cnt;
cv::Mat m_left_mat;
cv::Mat m_right_mat;
cv::Mat m_solved_mat;
}; // class Regression1D
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_REGRESSION_1D_H_INCLUDED

156
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm.h Executable file
View File

@@ -0,0 +1,156 @@
/*
* RobotFSM implements the state machine to explore and follow a line
* for sick_line_guidance_demo.
* The following RobotStates are executed:
* INITIAL -> EXPLORE_LINE -> FOLLOW_LINE [ -> WAIT_AT_BARCODE -> FOLLOW_LINE ] -> EXIT
* Theses states are implemented in the following classes:
* EXPLORE_LINE: ExploreLineState
* FOLLOW_LINE: FollowLineState
* WAIT_AT_BARCODE: WaitAtBarcodeState
*
* 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_DEMO_ROBOT_FSM_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_H_INCLUDED
#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include "sick_line_guidance/OLS_Measurement.h"
#include "sick_line_guidance_demo/explore_line_state.h"
#include "sick_line_guidance_demo/follow_line_state.h"
#include "sick_line_guidance_demo/robot_fsm_context.h"
#include "sick_line_guidance_demo/wait_at_barcode_state.h"
namespace sick_line_guidance_demo
{
/*
* class RobotFSM implements the state machine to explore and follow a line
* for sick_line_guidance_demo.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
class RobotFSM
{
public:
/*
* Constructor
* @param[in] nh ros handle
* @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input)
* @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input)
* @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output)
*/
RobotFSM(ros::NodeHandle* nh=0, const std::string & ros_topic_ols_messages = "/ols", const std::string & ros_topic_odometry = "/odom", const std::string & ros_topic_cmd_vel = "/cmd_vel");
/*
* Destructor
*/
~RobotFSM();
/*
* Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel
*/
void startFSM(void);
/*
* Stops the thread to run the final state machine
*/
void stopFSM(void);
/*
* message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom".
* It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot,
* and transform them invers into world coordinates.
* @param[in] msg odometry message (input)
*/
virtual void messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
/*
* message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols".
* It displays the OLS measurement (line info and barcodes), if visualization is enabled.
* @param[in] msg OLS measurement message (input)
*/
virtual void messageCallbackOlsMeasurement(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg);
protected:
/*
* thread callback, runs the final state machine for sick_line_guidance_demo.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
void runFSMthread(void);
/*
* member data: context and subscriber
*/
RobotFSMContext m_fsm_context; // shared context
ros::Subscriber m_ols_subscriber; // ros subscriber for ols messages (fsm input)
ros::Subscriber m_odom_subscriber; // ros subscriber for odom messages (fsm input)
ros::Publisher m_cmd_vel_publisher; // ros publisher for cmd_vel messages (fsm output)
/*
* member data: states and thread to run the state engine
*/
boost::thread* m_fsm_thread; // thread to run the state machine
bool m_fsm_thread_run; // true: m_fsm_thread is currently running, false: m_fsm_thread stopping
sick_line_guidance_demo::ExploreLineState m_explore_line; // state to explore a line in state RobotState::EXPLORE_LINE
sick_line_guidance_demo::FollowLineState m_follow_line; // state to follow a line in state RobotState::FOLLOW_LINE
sick_line_guidance_demo::WaitAtBarcodeState wait_at_barcode_state; // state to wait at a barcode in state RobotState::WAIT_AT_BARCODE
}; // class RobotFSM
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_H_INCLUDED

262
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/robot_fsm_context.h Executable file
View File

@@ -0,0 +1,262 @@
/*
* RobotFSMContext implements a threadsafe context of RobotFSM
* incl. ols, odom, and fsm state.
*
* 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_DEMO_ROBOT_FSM_CONTEXT_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_CONTEXT_H_INCLUDED
#include "sick_line_guidance/OLS_Measurement.h"
#include "sick_line_guidance_demo/navigation_util.h"
#include "sick_line_guidance_demo/set_get.h"
namespace sick_line_guidance_demo
{
/*
* Container for (x,y) position and yaw angle
*/
class RobotPosition
{
public:
RobotPosition(double x = 0, double y = 0, double a = 0) : pos(x,y), yaw(a) {}
cv::Point2d pos;
double yaw;
};
/*
* Threadsafe context of robot_fsm: ols, odom and fsm state
*/
class RobotFSMContext
{
public:
/*
* Enumerates the states of robot FSM: INITIAL -> EXPLORE_LINE -> FOLLOW_LINE [ -> WAIT_AT_BARCODE -> FOLLOW_LINE ] -> EXIT
*/
typedef enum RobotStateEnum
{
INITIAL,
EXPLORE_LINE,
FOLLOW_LINE,
WAIT_AT_BARCODE,
FATAL_ERROR,
EXIT
} RobotState;
/*
* Constructor
*/
RobotFSMContext() : m_velocity_publisher(0)
{
m_follow_left_turnout.set(false);
m_cur_barcode.set(0);
m_last_barcode.set(0);
}
/*
* set the current ols measurement
*/
void setOlsState(const sick_line_guidance::OLS_Measurement::ConstPtr& msg)
{
if(msg)
{
m_ols_msg.set(*msg);
setCurBarcode(sick_line_guidance_demo::NavigationUtil::barcode(*msg));
}
}
/*
* returns the current ols measurement
*/
sick_line_guidance::OLS_Measurement getOlsState(void)
{
return m_ols_msg.get();
}
/*
* returns the timestamp of current ols measurement
*/
ros::Time getOlsStateTime(void)
{
return m_ols_msg.getTime();
}
/*
* set the current odometry measurement
*/
void setOdomState(const nav_msgs::Odometry::ConstPtr& msg)
{
if(msg)
{
double posx=0, posy=0, yaw = 0;
sick_line_guidance_demo::NavigationUtil::toWorldPosition(msg, posx, posy, yaw);
RobotPosition odom_position(posx, posy, yaw);
setOdomPosition(odom_position);
}
}
/*
* set the current robot position from odometry measurement
*/
void setOdomPosition(const RobotPosition & odom_position)
{
m_odom_position.set(odom_position);
}
/*
* returns the current robot position from odometry
*/
RobotPosition getOdomPosition(void)
{
return m_odom_position.get();
}
/*
* returns the timestamp of current odometry measurement
*/
ros::Time getOdomPositionTime(void)
{
return m_odom_position.getTime();
}
/*
* set follow_left_turnout flag, true: follow a turnout on the left side, flag is toggled at barcode 101
*/
void setFollowLeftTurnout(bool follow_left_turnout)
{
m_follow_left_turnout.set(follow_left_turnout);
}
/*
* returns the current follow_left_turnout flag, true: follow a turnout on the left side, flag is toggled at barcode 101
*/
bool getFollowLeftTurnout(void)
{
return m_follow_left_turnout.get();
}
/*
* set the label of current barcode (0: no barcode detected)
*/
void setCurBarcode(uint32_t cur_barcode)
{
if(cur_barcode != 0 && cur_barcode != getLastBarcode())
m_last_barcode.set(cur_barcode);
m_cur_barcode.set(cur_barcode);
}
/*
* returns the label of current barcode (0: no barcode detected)
*/
uint32_t getCurBarcode(void)
{
return m_cur_barcode.get();
}
/*
* returns the label of last barcode (0: no barcode detected)
*/
uint32_t getLastBarcode(void)
{
return m_last_barcode.get();
}
/*
* returns true, if the last ols messages has been received more than <timeout_sec> seconds ago.
*/
bool hasOlsMessageTimeout(double timeout_sec)
{
ros::Time ols_time = getOlsStateTime();
return ols_time.isValid() && ((ros::Time::now() - ols_time).toSec() > timeout_sec);
}
/*
* returns true, if the last odom messages has been received more than <timeout_sec> seconds ago.
*/
bool hasOdomMessageTimeout(double timeout_sec)
{
ros::Time odom_time = getOdomPositionTime();
return odom_time.isValid() && ((ros::Time::now() - odom_time).toSec() > timeout_sec);
}
/*
* publishes a cmd_vel message
*/
void publish(geometry_msgs::Twist & velocityMessage)
{
if(m_velocity_publisher)
m_velocity_publisher->publish(velocityMessage);
}
void setVelocityPublisher(ros::Publisher* publisher)
{
m_velocity_publisher = publisher;
}
protected:
sick_line_guidance_demo::SetGet<sick_line_guidance::OLS_Measurement> m_ols_msg; // ols measurement message
sick_line_guidance_demo::SetGet<RobotPosition> m_odom_position; // robot position from odometry
sick_line_guidance_demo::SetGet<bool> m_follow_left_turnout; // true: follow a turnout on the left side, flag is toggled at barcode 101
sick_line_guidance_demo::SetGet<uint32_t> m_cur_barcode; // label of current barcode (0: no barcode detected)
sick_line_guidance_demo::SetGet<uint32_t> m_last_barcode; // label of last detected barcode (0: no barcode detected)
ros::Publisher* m_velocity_publisher;
};
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_ROBOT_FSM_CONTEXT_H_INCLUDED

116
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/set_get.h Executable file
View File

@@ -0,0 +1,116 @@
/*
* set_get implements an utility template class to set and get values threadsafe.
*
* 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_DEMO_SET_GET_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_SET_GET_H_INCLUDED
namespace sick_line_guidance_demo
{
/*
* class SetGet implements an utility template to set and get values threadsafe.
*/
template<typename T> class SetGet
{
public:
/*
* Constructor
*/
SetGet() : m_timestamp(0){}
/*
* Constructor
*/
SetGet(const T & value) { set(value); }
/*
* Set a value
*/
void set(const T & value)
{
boost::lock_guard<boost::mutex> state_lockguard(m_mutex);
m_value = value;
m_timestamp = ros::Time::now();
}
/*
* Get a value
*/
T get(void)
{
boost::lock_guard<boost::mutex> state_lockguard(m_mutex);
return m_value;
}
/*
* Returns the timestamp (i.e. the time a value has been set)
*/
ros::Time getTime(void)
{
boost::lock_guard<boost::mutex> state_lockguard(m_mutex);
return m_timestamp;
}
protected:
T m_value;
boost::mutex m_mutex;
ros::Time m_timestamp;
}; // class SetGet
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_SET_GET_H_INCLUDED

115
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/stop_go_fsm.h Executable file
View File

@@ -0,0 +1,115 @@
/*
* StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in stop and go.
*
* 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_DEMO_STOP_GO_FSM_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_STOP_GO_FSM_H_INCLUDED
#include <nav_msgs/Odometry.h>
namespace sick_line_guidance_demo
{
/*
* class StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in stop and go.
*/
class StopGoVelocityFSM
{
public:
/*
* Constructor
*/
StopGoVelocityFSM();
/*
* Destructor
*/
~StopGoVelocityFSM();
/*
* Next cycle, internal state is updated, velocity message may switch to next movement
*/
void update(void);
/*
* Returns the cmd_vel message for the current movement
*/
geometry_msgs::Twist getVelocity(void);
protected:
/*
* VelocityState := cmd_vel message and its duration
* class VelocityState is just a container for a cmd_vel message and its duration
*/
class VelocityState
{
public:
geometry_msgs::Twist cmd_vel;
ros::Duration duration;
};
/*
* member data
*/
int m_state_cnt;
ros::Time m_next_state_switch;
std::vector<VelocityState> m_vec_vel_states;
}; // class StopGoVelocityFSM
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_STOP_GO_FSM_H_INCLUDED

105
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/time_format.h Executable file
View File

@@ -0,0 +1,105 @@
/*
* time_format implements utility function to print the current time in different time formats.
* Note: By default, ROS_INFO, ROS_DEBUG and ROS_ERROR prints the walltime like '1563182704.466086322',
* while rosspy uses time format like '2019-07-15 12:23:24,859'.
* For easier debugging, TimeFormat prints both timestamp formats.
*
* 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_DEMO_TIME_FORMAT_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_TIME_FORMAT_H_INCLUDED
#include "boost/date_time/posix_time/posix_time.hpp"
namespace sick_line_guidance_demo
{
/*
* class TimeFormat
*/
class TimeFormat
{
public:
/*
* Returns current date and time in a format like "[2019-07-15 12:23:24,859123] "
*/
static std::string formatDateTime(void)
{
boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%d %H:%M:%S,%f");
time_t rawtime;
time(&rawtime);
ros::WallTime local_walltime = ros::WallTime::now() + ros::WallDuration(localtime(&rawtime)->tm_gmtoff);
std::stringstream timestamp;
timestamp.imbue(std::locale(timestamp.getloc(), facet));
timestamp << "[" << local_walltime.toBoost() << "] ";
return timestamp.str();
}
/*
* Returns current date and time in a format like "[2019-07-15 12:23:24,859123] "
*/
static std::string formatDateTime(const ros::Time & time)
{
boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%d %H:%M:%S,%f");
std::stringstream timestamp;
timestamp.imbue(std::locale(timestamp.getloc(), facet));
timestamp << time.toBoost();
return timestamp.str();
}
}; // class TimeFormat
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_TIME_FORMAT_H_INCLUDED

143
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/turtlebot_test_fsm.h Executable file
View File

@@ -0,0 +1,143 @@
/*
* TurtlebotTestFSM implements a state machine to generate cmd_vel messages
* with varying velocities to test the motor control of a TurtleBot.
*
* 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_DEMO_TURTLEBOT_TEST_FSM_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_TURTLEBOT_TEST_FSM_H_INCLUDED
#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include "sick_line_guidance/OLS_Measurement.h"
namespace sick_line_guidance_demo
{
/*
* class TurtlebotTestFSM implements a state machine to generate cmd_vel messages
* with varying velocities to test the motor control of a TurtleBot.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
class TurtlebotTestFSM
{
public:
/*
* Constructor
* @param[in] nh ros handle
* @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input)
* @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input)
* @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output)
* @param[in] print_errors true (default): print high latency by error message (print by info message otherwise)
*/
TurtlebotTestFSM(ros::NodeHandle* nh=0, const std::string & ros_topic_ols_messages = "/ols", const std::string & ros_topic_odometry = "/odom", const std::string & ros_topic_cmd_vel = "/cmd_vel", bool print_errors = true);
/*
* Destructor
*/
~TurtlebotTestFSM();
/*
* Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel
*/
void startFSM(void);
/*
* Stops the thread to run the final state machine
*/
void stopFSM(void);
/*
* message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom".
* It compares odom messages with the current velocity from last cmd_vel command and measures latencies to stop the Turtlebot after moving
* with linear and angular velocitiy.
* @param[in] msg odometry message (input)
*/
virtual void messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
/*
* message callback for OLS measurement messages, empty function
*/
virtual void messageCallbackOlsMeasurement(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg);
protected:
/*
* thread callback, runs the final state machine.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
void runFSMthread(void);
/*
* member data
*/
ros::Subscriber m_ols_subscriber; // ros subscriber for ols messages (fsm input)
ros::Subscriber m_odom_subscriber; // ros subscriber for odom messages (fsm input)
ros::Publisher m_cmd_vel_publisher; // ros publisher for cmd_vel messages (fsm output)
ros::Rate m_cmd_vel_publish_rate; // cmd_vel messages are published with 20 Hz by default
boost::thread* m_fsm_thread; // thread to run the state machine
bool m_fsm_thread_run; // true: m_fsm_thread is currently running, false: m_fsm_thread stopping
nav_msgs::Odometry m_cur_odom; // last received odom message
ros::Time m_cur_odom_timestamp; // time of last received odom message
geometry_msgs::Twist m_cur_velocity; // last published cmd_vel message
ros::Time m_cur_velocity_timestamp; // time of last published cmd_vel message
ros::Time m_timestamp_stopped; // time of last stop
bool m_print_errors; // true (default): print high latency by error message (print by info message otherwise)
}; // class TurtlebotTestFSM
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_TURTLEBOT_TEST_FSM_H_INCLUDED

134
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/velocity_ctr.h Executable file
View File

@@ -0,0 +1,134 @@
/*
* velocity_ctr controls angular velocity to avoid acceleration and changing velocity.
* velocity_ctr increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached.
*
* 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 __ROBOT_FSM_VELOCITY_CTR_H_INCLUDED
#define __ROBOT_FSM_VELOCITY_CTR_H_INCLUDED
namespace sick_line_guidance_demo
{
/*
* class AngularZCtr controls angular velocity to avoid acceleration and changing velocity.
* It increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached.
*/
class AngularZCtr
{
public:
/*
* Constructor
*/
AngularZCtr();
/*
* Destructor
*/
virtual ~AngularZCtr();
/*
* Starts a new control cycle with smooth transition from cur_yaw_angle and velocity.angular.z = 0 to
* dest_yaw_angle and dest_vel_angular_z.
* @param[in] cur_yaw_angle: current yaw angle (robots current heading)
* @param[in] dest_yaw_angle: destination yaw angle (robots desired heading)
* @param[in] dest_vel_angular_z: destination velocity.angular.z
* @param[in] line_detected: if true (default), start with 5 cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z, otherwise there's only one cycle with velocity.angular.z=0
* @return first value of velocity.angular.z after start, default: 0
*/
virtual double start(double cur_yaw_angle, double dest_yaw_angle, double dest_vel_angular_z, bool line_detected = true);
/*
* Stops angular.z control, if currently running.
*/
virtual void stop();
/*
* Update velocity.angular.z control with the current yaw angle.
* @param[in] cur_yaw_angle: current yaw angle (robots current heading)
*/
virtual void update(double cur_yaw_angle);
/*
* Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until
* dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z)
* is reached.
*/
virtual bool isRunning(void);
/*
* Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until
* dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z)
* is reached. In this case, output parameter angular_z is set to the new velocity.angular.z to be published,
* and true is returned. Otherwise, false is returned and angular_z remains unchanged.
* @param[out] angular_z: velocity.angular.z to be published (if true returned), otherwise left untouched.
*/
virtual bool isRunning(double & angular_z);
protected:
double m_dest_yaw_angle; // robots desired heading
double m_dest_vel_angular_z; // desired velocity.angular.z (max. value to increase/decrease m_cur_vel_angular_z)
double m_cur_yaw_angle; // robots current heading
double m_cur_vel_angular_z; // current velocity.angular.z (increased resp. decreased from 0.0 to m_dest_vel_angular_z)
double m_start_yaw_angle; // robots heading at start time
ros::Time m_start_time; // timestamp at start
int m_update_cnt; // just counts the number of updates (cycles) required to reach the destination heading, or -1 if not started
int m_number_start_cyles_with_zero_angular_z; // number of start cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z (default: 5)
double m_delta_angular_z; // increase/decrease velocity.angular.z by m_delta_angular_z until delta_angle_now is smaller than some epsilon (default: 0.05 * M_PI / 4)
}; // class AngularZCtr
} // namespace sick_line_guidance_demo
#endif // __ROBOT_FSM_VELOCITY_CTR_H_INCLUDED

113
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/include/sick_line_guidance_demo/wait_at_barcode_state.h Executable file
View File

@@ -0,0 +1,113 @@
/*
* WaitAtBarcodeState implements the state to wait at a barcode for a configurable amount of time.
*
* 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_DEMO_WAIT_AT_BARCODE_STATE_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DEMO_WAIT_AT_BARCODE_STATE_H_INCLUDED
#include <boost/thread.hpp>
#include <nav_msgs/Odometry.h>
#include "sick_line_guidance_demo/robot_fsm_context.h"
namespace sick_line_guidance_demo
{
/*
* WaitAtBarcodeState implements the state to wait at a barcode for a configurable amount of time.
*/
class WaitAtBarcodeState
{
public:
/*
* Constructor
* @param[in] nh ros handle
* @param[in] context shared fsm context
*/
WaitAtBarcodeState(ros::NodeHandle* nh=0, RobotFSMContext* context = 0);
/*
* Destructor
*/
~WaitAtBarcodeState();
/*
* Clears all internal states
*/
void clear(void);
/*
* Runs the wait at barcode state for a configurable amount of time.
* Toggles follow_left_turnout flag at barcode label 101.
* @return FOLLOW_LINE, or EXIT in case ros::ok()==false.
*/
RobotFSMContext::RobotState run(void);
protected:
/*
* member data
*/
RobotFSMContext* m_fsm_context; // shared state context
/*
* Configuration
*/
double m_stop_wait_time; // time in seconds to stop at a barcode, default: 10 seconds
}; // class WaitAtBarcodeState
} // namespace sick_line_guidance_demo
#endif // __SICK_LINE_GUIDANCE_DEMO_WAIT_AT_BARCODE_STATE_H_INCLUDED

11
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/Robot_FSM_demo.launch Executable file
View File

@@ -0,0 +1,11 @@
<launch>
<rosparam command="load" file="$(find iam)/AGC.yaml"/>
<!-- include file="$(find turtlebot3_bringup)/launch/turtlebot3_robot.launch"/ -->
<!-- include file="$(find sick_line_guidance)/sick_line_guidance.launch"/>
<arg name="yaml" value="sick_line_guidance_ols20.yaml"/>
</include -->
<!-- node pkg="agc_radar" type="agc_radar" name="agc_radar"/ -->
<!-- node pkg="lidar_obstacle_detection" type="obstacle_detector" name="obstacle_detector" output="screen"/ -->
<!-- node pkg="iam" type="robot_fsm" name="fsm" output="screen" launch-prefix="xterm -fa 'Monospace' -fs 11 -e gdb -ex run - - args" / -->
<node pkg="iam" type="robot_fsm" name="fsm" output="screen"/>
</launch>

22
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_demo_node.launch Executable file
View File

@@ -0,0 +1,22 @@
<launch>
<!-- sick_line_guidance_demo: run sick_line_guidance_demo_node, which simulates sick_line_guidance_demo -->
<arg name="ols_simu"/>
<arg name="visualize"/>
<rosparam command="load" file="$(find sick_line_guidance)/sick_line_guidance_demo.yaml"/>
<node name="sick_line_guidance_demo_node" pkg="sick_line_guidance" type="sick_line_guidance_demo_node" >
<param name="ols_simu" type="int" value="$(arg ols_simu)" /> <!-- simulate ols measurement messages -->
<param name="visualize" type="int" value="$(arg visualize)" /> <!-- visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled. -->
<param name="ros_topic_odometry" type="str" value="/odom" /> <!-- ROS topic for odometry incl. robot positions (input) -->
<param name="ros_topic_ols_messages" type="str" value="/ols" /> <!-- ROS topic for OLS_Measurement messages (simulation: input+output, otherwise input) -->
<param name="ros_topic_cmd_vel" type="str" value="/cmd_vel" /> <!-- ROS topic for cmd_vel messages (ouput) -->
<param name="navigation_map_file" type="str" value="$(find sick_line_guidance)/demo_map_02.png" /> <!-- Navigation map (input) -->
<param name="intrinsics_xml_file" type="str" value="$(find sick_line_guidance)/cam_intrinsic.xml" /> <!-- Intrinsics parameter to transform position from navigation map (pixel) to real world (meter) and vice versa (input, cx=cy=660, fx=fy=1) -->
<param name="barcodes_xml_file" type="str" value="$(find sick_line_guidance)/demo_barcodes.xml" /> <!-- List of barcodes with label and position (input) -->
<param name="line_sensor_detection_width" type="double" value="0.130" /> <!-- Default: 0.130, Width of the detection area of the line sensor (meter) -->
<param name="line_sensor_scaling_dist" type="double" value="1.35" /> <!-- Default: 180.0/133.0, Scaling between physical distance to the line center and measured line center point (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height -->
<param name="line_sensor_scaling_width" type="double" value="1.45" /> <!-- Default: 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) -->
<param name="line_sensor_mounted_right_to_left" type="bool" value="true" /> <!-- Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right -->
</node>
</launch>

13
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/sick_line_guidance_watchdog.launch Executable file
View File

@@ -0,0 +1,13 @@
<launch>
<!-- sick_line_guidance_demo: run sick_line_guidance_watchdog, to monitor ols messages for missing lines and barcode and to run an emergency script to stop the TurtleBot after watchdog timeout -->
<node name="sick_line_guidance_watchdog" pkg="sick_line_guidance" type="sick_line_guidance_watchdog" >
<param name="ols_topic" type="str" value="/ols" /> <!-- ROS topic for OLS_Measurement messages -->
<param name="odom_topic" type="str" value="/odom" /> <!-- ROS topic for odometry messages -->
<param name="watchdog_timeout" type="double" value="1.0" /> <!-- watchdog timeout in seconds -->
<param name="watchdog_check_frequency" type="double" value="10" /> <!-- rate to check OLS messages -->
<param name="barcode_height" type="double" value="0.055" /> <!-- height of barcode (55 mm, area without valid ols line) -->
<param name="watchdog_command" type="str" value="rosnode kill fsm ; sleep 0.5 ; rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' ; sleep 0.5 ; nohup rosnode kill -a" /> <!-- watchdog command, executed in case of watchdog timeouts -->
</node>
</launch>

6
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/launch/turtlebot_test_fsm_node.launch Executable file
View File

@@ -0,0 +1,6 @@
<launch>
<!-- turtlebot_test_fsm_node: simple turtlebot test, drives a figure of eight -->
<node name="turtlebot_test_fsm_node" pkg="sick_line_guidance" type="turtlebot_test_fsm_node" />
</launch>

13
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/cam_intrinsic.xml Executable file
View File

@@ -0,0 +1,13 @@
<?xml version="1.0"?>
<opencv_storage>
<Camera_Matrix type_id="opencv-matrix">
<rows>3</rows>
<cols>3</cols>
<dt>d</dt>
<data>
1000. 0. 600.
0. -1000. 600.
0. 0. 1.
</data>
</Camera_Matrix>
</opencv_storage>

7
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/maps/demo_barcodes.xml Executable file
View File

@@ -0,0 +1,7 @@
<?xml version="1.0"?>
<sick_line_guidance_demo>
<barcodes> <!-- list of barcodes with label and position in world coordinates (meter) -->
<barcode label="0101" outer_pos_x1="+0.318" outer_pos_y1="-0.0275" outer_pos_x2="+0.379" outer_pos_y2="+0.0275" inner_pos_x1="+0.318" inner_pos_y1="-0.0105" inner_pos_x2="+0.379" inner_pos_y2="+0.0245" flipped="false" />
<barcode label="0102" outer_pos_x1="-0.252" outer_pos_y1="-0.0275" outer_pos_x2="-0.191" outer_pos_y2="+0.0275" inner_pos_x1="-0.252" inner_pos_y1="-0.0105" inner_pos_x2="-0.191" inner_pos_y2="+0.0245" flipped="true" />
</barcodes>
</sick_line_guidance_demo>

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

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 29 KiB

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

@@ -0,0 +1,81 @@
<?xml version="1.0"?>
<package format="2">
<name>sick_line_guidance_demo</name>
<version>0.0.0</version>
<description>SICK Line Guidance ROS Demo</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<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://github.com/SICKAG/sick_line_guidance_demo</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 -->
<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>nav_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>random_numbers</build_depend>
<build_depend>sick_line_guidance</build_depend>
<build_depend>sm_timing</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>nav_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>sick_line_guidance</build_export_depend>
<build_export_depend>sm_timing</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>random_numbers</exec_depend>
<exec_depend>sick_line_guidance</exec_depend>
<exec_depend>sm_timing</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

315
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/adjust_heading.cpp Executable file
View File

@@ -0,0 +1,315 @@
/*
* AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time.
* It searches the line orientation by minimizing the line distance and adjusts the robot heading.
*
* Algorithm:
* - Stop the robot, set linear velocity to 0.0
* - Minimize the line distance by rotating the robot:
* + Start rotating clockwise
* + If the lined distance increases, rotate anti-clockwise
* + Stop rotation when the line distance is minimal.
* The robot is now orientated parallel to the line.
* - Reset PID and continue moving along the line.
*
* 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 <string>
#include <vector>
#include <ros/ros.h>
#include "sick_line_guidance_demo/adjust_heading.h"
/*
* class AdjustHeading implements a state machine to adjust the robot heading, if the line distance increases over time.
* It searches the line orientation by minimizing the line distance and adjusts the robot heading.
* Algorithm:
* - Stop the robot, set linear velocity to 0.0
* - Minimize the line distance by rotating the robot:
* + Start rotating clockwise
* + If the lined distance increases, rotate anti-clockwise
* + Stop rotation when the line distance is minimal.
* The robot is now orientated parallel to the line.
* - Reset PID and continue moving along the line.
*/
/*
* Constructor
*/
sick_line_guidance_demo::AdjustHeading::AdjustHeading()
: m_adjust_heading_state(ADJUST_HEADING_STATE_STOPPED), m_use_line_width(false), m_line_distance_at_start(0), m_line_width_at_start(0),
m_line_distance_best(0), m_line_width_best(0), m_feature_value_best(0), m_yaw_angle_best(0), m_final_step_delta_heading(0), m_final_step_toggle_cnt(0)
{
}
/*
* Destructor
*/
sick_line_guidance_demo::AdjustHeading::~AdjustHeading()
{
}
/*
* Starts a new adjustment.
* @param[in] line_distance: current distance between robot and line
* @param[in] yaw_angle: current yaw angle from odometry
* @param[in] use_line_width: minimize line distance (false,default) or line width (true)
* @param[in] search_lower_bound: if the current line distance is below search_lower_bound, the search will stop
* @param[in] search_upper_bound: if the current line distance is above search_upper_bound, the search will continue in opposite direction (max. detection zone: +- 65 mm)
* @param[in] angular_z: velocity.angular.z under adjustment, default: 0.1 * M_PI / 4
* @param[in] measurement_jitter: tolerate some line measurement jitter when adjusting the heading, default: 0.003
* @param[in] lcp_deviation_thresh: stop heading adjustment, if the line distance finally increases again (lcp-deviation above lcp_deviation_thresh)
*/
/*
* Starts a new adjustment.
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry
* @param[in] use_line_width: minimize line distance (false,default) or line width (true)
* @param[in] adjust_heading_cfg Parameter (upper and lower bounds) configuring the search
*/
void sick_line_guidance_demo::AdjustHeading::start(double line_distance, double line_width, double yaw_angle, bool use_line_width, const AdjustHeadingConfig & adjust_heading_cfg)
{
m_adjust_heading_cfg = adjust_heading_cfg; // Parameter (upper and lower bounds) configuring the search
m_use_line_width = use_line_width; // minimize line distance (false,default) or line width (true)
m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_FORWARD_1; // forward search until upper bound reached (1. path)
m_final_step_delta_heading = 0; // delta angle at final step (:= <current yaw angle> - m_yaw_angle_best)
m_final_step_toggle_cnt = 0; // counts possible toggling around the minimum line distance at the final optimization step
m_yaw_angle_best = yaw_angle; // yaw angle at lowest line distance
m_line_distance_at_start = line_distance; // line distance at start of search
m_line_width_at_start = line_width; // line width at start of search
m_yaw_angle_at_start = yaw_angle; // yaw angle at start of search
m_line_distance_best = line_distance; // lowest line distance found while searching
m_line_width_best = line_width; // lowest line width found while searching
m_feature_value_best = computeFeature(line_distance, line_width, yaw_angle); // lowest feature value found while searching
m_wait_at_state_transition.start(yaw_angle, yaw_angle, 0);
for(size_t n = 0; n < ADJUST_HEADING_MAX_STATES; n++)
m_states_start_time[n] = ros::Time(0);
m_states_start_time[m_adjust_heading_state] = ros::Time::now(); // test only: limit time amount in seconds to stay in current search state; after <max_state_duration> seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions
}
/*
* Stops a running adjustment.
*/
void sick_line_guidance_demo::AdjustHeading::stop(void)
{
m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED;
}
/*
* Returns true, if an adjustment is currently running.
*/
bool sick_line_guidance_demo::AdjustHeading::isRunning(void)
{
return (m_adjust_heading_state != ADJUST_HEADING_STATE_STOPPED) || m_wait_at_state_transition.isRunning();
}
/*
* Returns true, if line distance or line width are above their upper bounds. In this case, search direction is inverted. Otherwise false is returned.
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry (currently unused)
* @return true, if measurement (line distance or line width) out of configured bounds
*/
bool sick_line_guidance_demo::AdjustHeading::greaterThanUpperBounds(double line_distance, double line_width, double yaw_angle)
{
return ((std::abs(line_distance) >= m_adjust_heading_cfg.search_upper_bound_linedistance) // line distance beyond limit
|| (std::abs(line_width) >= m_adjust_heading_cfg.search_upper_bound_linewidth) // line width beyond limit
|| (std::abs(deltaAngle(yaw_angle, m_yaw_angle_at_start)) >= m_adjust_heading_cfg.search_max_yaw_angle_delta) // yaw angle beyond limit
|| ((ros::Time::now() - m_states_start_time[m_adjust_heading_state]).toSec() > m_adjust_heading_cfg.max_state_duration)); // timeout
}
/*
* Computes the feature value from a new measurement.
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry (currently unused)
* @return feature value
*/
double sick_line_guidance_demo::AdjustHeading::computeFeature(double line_distance, double line_width, double yaw_angle)
{
if(m_use_line_width) // minimize combination of line_distance and line_width
{
double feature_dist = line_distance / m_adjust_heading_cfg.sigma_linedistance;
double feature_width = line_width / m_adjust_heading_cfg.sigma_linewidth;
return std::sqrt(feature_dist * feature_dist + feature_width * feature_width);
}
return line_distance; // minimize line distance
}
/*
* Updates the current line distance and switches the state, if an upper bound or the minimum line distance has been reached:
* ADJUST_HEADING_STATE_STOPPED -> ADJUST_HEADING_STATE_SEARCH_FORWARD_1 -> ADJUST_HEADING_STATE_SEARCH_BACKWARD-2 -> ADJUST_HEADING_STATE_SEARCH_FORWARD_3 -> ADJUST_HEADING_STATE_STOPPED
* @param[in] line_detected: true if line detected (line_distance is valid), false if no line detected
* @param[in] line_distance: current distance between robot and line
* @param[in] line_width: current line width
* @param[in] yaw_angle: current yaw angle from odometry
* @return angular velocity (updated value, taken the current line_distance into account)
*/
double sick_line_guidance_demo::AdjustHeading::update(bool line_detected, double sensor_line_distance, double sensor_line_width, double yaw_angle)
{
// Compute minimization feature (line distance or a combination of line distance and line width
double angular_z = 0;
double feature_value = computeFeature(sensor_line_distance, sensor_line_width, yaw_angle);
double feature_value_at_start = computeFeature(m_line_distance_at_start, m_line_width_at_start, m_yaw_angle_at_start);
AdjustHeadingAutoPrinter auto_printer(this, line_detected, sensor_line_distance, sensor_line_width, feature_value, yaw_angle, &angular_z);
// Update velocity control for state transition with smooth angular_z
if(!line_detected)
m_wait_at_state_transition.stop();
else
m_wait_at_state_transition.update(yaw_angle);
if(!isRunning())
return angular_z;
// Testmode: limit time amount in seconds to stay in current search state; after <max_state_duration> seconds state increases.
// Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions
if(!m_states_start_time[m_adjust_heading_state].isValid())
m_states_start_time[m_adjust_heading_state] = ros::Time::now();
// Stop search if lower bound reached (it won't get better)
if(line_detected && std::abs(sensor_line_distance) <= m_adjust_heading_cfg.search_lower_bound_linedistance + 0.001 && std::abs(sensor_line_width) <= m_adjust_heading_cfg.search_lower_bound_linewidth + 0.0001)
{
if(m_adjust_heading_state != ADJUST_HEADING_STATE_STOPPED)
{
m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED;
angular_z = m_wait_at_state_transition.start(yaw_angle, yaw_angle, 0, line_detected);
}
return angular_z;
}
// Update best feature value found while searching
if(line_detected && std::abs(feature_value) < std::abs(m_feature_value_best))
{
m_feature_value_best = feature_value;
m_line_distance_best = sensor_line_distance;
m_line_width_best = sensor_line_width;
m_yaw_angle_best = yaw_angle;
}
// While waiting at state transitions: increase/decrease angular_z slowly until the desired yaw angle is reached. Avoid acceleration, smooth changes of velocity
if(m_wait_at_state_transition.isRunning(angular_z))
return angular_z;
angular_z = 0;
// 1. forward search: if line distance is out of its upper bound, revert search direction (i.e. invert m_heading_angular_z)
if(m_adjust_heading_state == ADJUST_HEADING_STATE_SEARCH_FORWARD_1)
{
if(!line_detected || greaterThanUpperBounds(sensor_line_distance, sensor_line_width, yaw_angle))
{
// line distance or line width out of bounds, revert search direction
m_adjust_heading_cfg.angular_z = -m_adjust_heading_cfg.angular_z;
m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_BACKWARD_2; // default: backward search after forward search
angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, m_adjust_heading_cfg.angular_z, line_detected); // increase angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity
return angular_z;
}
// forward search: just continue rotation with m_heading_angular_z
angular_z = (std::abs(feature_value) >= feature_value_at_start) ? 2 * m_adjust_heading_cfg.angular_z : m_adjust_heading_cfg.angular_z;
return angular_z;
}
// 2. backward search: if line distance is out of its upper bound, revert search direction (i.e. invert m_heading_angular_z)
if(m_adjust_heading_state == ADJUST_HEADING_STATE_SEARCH_BACKWARD_2)
{
if(!line_detected)
{
// line lost, revert search direction
m_adjust_heading_cfg.angular_z = -m_adjust_heading_cfg.angular_z;
m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_FORWARD_3;
angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, m_adjust_heading_cfg.angular_z, line_detected); // increase angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity
return angular_z;
}
if(greaterThanUpperBounds(sensor_line_distance, sensor_line_width, yaw_angle))
{
// line distance or line width out of bounds, revert search direction
m_adjust_heading_cfg.angular_z = -m_adjust_heading_cfg.angular_z;
m_adjust_heading_state = ADJUST_HEADING_STATE_SEARCH_FORWARD_3;
angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, m_adjust_heading_cfg.angular_z, line_detected); // increase angular_z slowly until the desired yaw angle is reached, avoid acceleration and change in velocity
return angular_z;
}
// backward search: just continue rotation with m_heading_angular_z
angular_z = (std::abs(feature_value) >= feature_value_at_start) ? 2 * m_adjust_heading_cfg.angular_z : m_adjust_heading_cfg.angular_z;
return angular_z;
}
// 3. backward search: continue with rotation until upper bound reached, or close to minimum
if(m_adjust_heading_state == ADJUST_HEADING_STATE_SEARCH_FORWARD_3)
{
double delta_heading = deltaAngle(yaw_angle, m_yaw_angle_best);
if(std::abs(delta_heading) < m_adjust_heading_cfg.delta_angle_epsilon) // stop search, best angular_z at lowest line distance reached
{
m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED;
angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, 0, line_detected);
return angular_z;
}
// Prevent toggling around the minimum line distance at the final optimization step
if(signum(m_final_step_delta_heading, m_adjust_heading_cfg.delta_angle_epsilon) != signum(delta_heading, m_adjust_heading_cfg.delta_angle_epsilon))
m_final_step_toggle_cnt++;
if(m_final_step_toggle_cnt > 4 || ((ros::Time::now() - m_states_start_time[m_adjust_heading_state]).toSec() > m_adjust_heading_cfg.max_state_duration)) // timeout
{
m_adjust_heading_state = ADJUST_HEADING_STATE_STOPPED; // stop search, toggling around the minimum
angular_z = m_wait_at_state_transition.start(yaw_angle, m_yaw_angle_best, 0, line_detected);
return angular_z;
}
// continue until closer to the minimum
m_final_step_delta_heading = delta_heading;
if(delta_heading >= 0)
angular_z = std::max(delta_heading, std::abs(m_adjust_heading_cfg.angular_z));
else
angular_z = std::min(delta_heading, -std::abs(m_adjust_heading_cfg.angular_z));
return angular_z; // velocity.angular.z = delta_heading per second, i.e. we turn slowly back (desired heading reached in one second), avoiding acceleration
}
return 0;
}

118
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/barcodes.cpp Executable file
View File

@@ -0,0 +1,118 @@
/*
* barcodes implements a container for barcodes for sick_line_guidance_demo (position and label).
*
* 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/string.hpp>
#include <cstdlib>
#include <string>
#include <vector>
#include <tinyxml.h>
#include <ros/ros.h>
#include <opencv2/core.hpp>
#include "sick_line_guidance_demo/barcodes.h"
/*
* reads a list of barcodes from xml-file
* @param[in] barcode_xml_file xml-file, f.e. "demo_barcodes.xml"
* @return list of barcodes (empty list, if xmlfile could not be read)
*/
std::vector<sick_line_guidance_demo::Barcode> sick_line_guidance_demo::BarcodeUtil::readBarcodeXmlfile(const std::string & barcode_xml_file)
{
std::vector<Barcode> barcodes;
// Read list of barcode settings from xml-file. Example ("demo_barcodes.xml"):
// <sick_line_guidance_demo>
// <barcodes> <!-- list of barcodes with label and position in world coordinates (meter) -->
// <barcode label="0101" pos_x1="+0.318" pos_y1="-0.0275" pos_x2="+0.379" pos_y2="+0.0275" flipped="false" />
// <barcode label="0102" pos_x1="-0.252" pos_y1="-0.0275" pos_x2="-0.191" pos_y2="+0.0275" flipped="true" />
// </barcodes>
// </sick_line_guidance_demo>
try
{
TiXmlDocument xml_config(barcode_xml_file);
if(xml_config.LoadFile())
{
TiXmlElement* xml_barcodes = xml_config.FirstChild("sick_line_guidance_demo")->FirstChild("barcodes")->ToElement();
if(xml_barcodes)
{
TiXmlElement* xml_barcode = xml_barcodes->FirstChildElement("barcode");
while(xml_barcode)
{
Barcode barcode;
barcode.label() = xml_barcode->Attribute("label");
double pos_x1 = std::stod(xml_barcode->Attribute("outer_pos_x1"));
double pos_y1 = std::stod(xml_barcode->Attribute("outer_pos_y1"));
double pos_x2 = std::stod(xml_barcode->Attribute("outer_pos_x2"));
double pos_y2 = std::stod(xml_barcode->Attribute("outer_pos_y2"));
barcode.outerRectWorld() = cv::Rect2d(pos_x1, pos_y1, pos_x2 - pos_x1, pos_y2 - pos_y1);
pos_x1 = std::stod(xml_barcode->Attribute("inner_pos_x1"));
pos_y1 = std::stod(xml_barcode->Attribute("inner_pos_y1"));
pos_x2 = std::stod(xml_barcode->Attribute("inner_pos_x2"));
pos_y2 = std::stod(xml_barcode->Attribute("inner_pos_y2"));
barcode.innerRectWorld() = cv::Rect2d(pos_x1, pos_y1, pos_x2 - pos_x1, pos_y2 - pos_y1);
std::string flipped = xml_barcode->Attribute("flipped");
barcode.flipped() = (boost::iequals(flipped, "true") || boost::iequals(flipped.substr(0,1), "y") || std::atoi(flipped.c_str()) > 0);
barcodes.push_back(barcode);
xml_barcode = xml_barcode->NextSiblingElement("barcode");
}
return barcodes;
}
}
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("readBarcodeXmlfile("<< barcode_xml_file << ") failed: exception " << exc.what());
}
return std::vector<Barcode>();
}

162
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/explore_line_state.cpp Executable file
View File

@@ -0,0 +1,162 @@
/*
* ExploreLineState implements the state to explore a line for sick_line_guidance_demo.
* As long as ols does not detect a line, cmd_vel messages are published to search a line.
* Currently, the TurtleBot just moves straight forwared until a line is detected.
* Input: ols messages
* Output: cmd_vel 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_demo/explore_line_state.h"
#include "sick_line_guidance_demo/navigation_util.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* Constructor
* @param[in] nh ros handle
* @param[in] context shared fsm context
*/
sick_line_guidance_demo::ExploreLineState::ExploreLineState(ros::NodeHandle* nh, RobotFSMContext* context) : m_fsm_context(context), m_explore_line_rate(20)
{
if(nh)
{
// Configuration of explore line state
double exploreLineRate;
ros::param::getCached("/explore_line_state/exploreLineRate", exploreLineRate); // frequency to update explore line state, default: 20 Hz
ros::param::getCached("/explore_line_state/exploreSpeed", m_exploreSpeed); // default linear velocity to explore a line
ros::param::getCached("/explore_line_state/olsMessageTimeout", m_ols_message_timeout); // timeout for ols messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
ros::param::getCached("/explore_line_state/odomMessageTimeout", m_odom_message_timeout); // timeout for odom messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
ros::Rate m_explore_line_rate = ros::Rate(exploreLineRate); // frequency to update explort line state, default: 20 Hz
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration explore line state: "
<< " exploreLineRate=" << exploreLineRate << " exploreSpeed=" << m_exploreSpeed
<< ", olsMessageTimeout=" << m_ols_message_timeout << ", odomMessageTimeout=" << m_odom_message_timeout);
}
}
/*
* Destructor
*/
sick_line_guidance_demo::ExploreLineState::~ExploreLineState()
{
m_fsm_context = 0;
}
/*
* Clears all internal states
*/
void sick_line_guidance_demo::ExploreLineState::clear(void)
{
}
/*
* Runs the explore line state until line is detected (or a fatal error occures).
* @return FOLLOW_LINE in case of line detected, or EXIT in case ros::ok()==false.
*/
sick_line_guidance_demo::RobotFSMContext::RobotState sick_line_guidance_demo::ExploreLineState::run(void)
{
assert(m_fsm_context);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: entering ExploreLineState");
geometry_msgs::Twist velocityMessage;
velocityMessage.linear.x = 0;
velocityMessage.linear.y = 0;
velocityMessage.linear.z = 0;
velocityMessage.angular.x = 0;
velocityMessage.angular.y = 0;
velocityMessage.angular.z = 0;
while(ros::ok())
{
m_explore_line_rate.sleep();
// Get current ols measurement
sick_line_guidance::OLS_Measurement ols_msg = m_fsm_context->getOlsState();
//sick_line_guidance_demo::RobotPosition odom_position = m_fsm_context->getOdomPosition();
bool ols_message_missing = m_fsm_context->hasOlsMessageTimeout(m_ols_message_timeout);
bool odom_message_missing = m_fsm_context->hasOdomMessageTimeout(m_odom_message_timeout);
if(ols_message_missing)
ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no ols message received since " << ((ros::Time::now() - m_fsm_context->getOlsStateTime()).toSec()) << " seconds");
if(odom_message_missing)
ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no odom message received since " << ((ros::Time::now() - m_fsm_context->getOdomPositionTime()).toSec()) << " seconds");
if(ols_message_missing || odom_message_missing)
{
velocityMessage.angular.z = 0;
velocityMessage.linear.x = 0;
m_fsm_context->publish(velocityMessage);
continue;
}
// Switch to FOLLOW_LINE, if line detected
if(sick_line_guidance_demo::NavigationUtil::lineDetected(ols_msg))
{
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: line detected, switching to state FOLLOW_LINE");
velocityMessage.angular.z = 0;
velocityMessage.linear.x = 0;
m_fsm_context->publish(velocityMessage);
return RobotFSMContext::RobotState::FOLLOW_LINE;
}
// Otherwise move straight forward
if(velocityMessage.linear.x < m_exploreSpeed)
{
velocityMessage.linear.x += m_exploreSpeed / 30.0;
}
else
{
velocityMessage.linear.x = m_exploreSpeed;
}
m_fsm_context->publish(velocityMessage);
}
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: leaving ExploreLineState");
return RobotFSMContext::RobotState::EXIT; // ros::ok() == false
}

158
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/figure_eight_fsm.cpp Executable file
View File

@@ -0,0 +1,158 @@
/*
* FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in a figure of eight.
*
* 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 <string>
#include <vector>
#include <opencv2/core.hpp>
#include <ros/ros.h>
#include "sick_line_guidance_demo/figure_eight_fsm.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* class FigureEightVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in a figure of eight.
*/
/*
* Constructor
*/
sick_line_guidance_demo::FigureEightVelocityFSM::FigureEightVelocityFSM()
{
// define the state machine
m_state_cnt = 0;
m_next_state_switch = ros::Time(0);
VelocityState state;
state.cmd_vel.linear.x = 0;
state.cmd_vel.linear.y = 0;
state.cmd_vel.linear.z = 0;
state.cmd_vel.angular.x = 0;
state.cmd_vel.angular.y = 0;
state.cmd_vel.angular.z = 0;
// 1. state: drive anti-clockwise half circle witch 180 degree turn in 5 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(5.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.01;
m_vec_vel_states.back().cmd_vel.angular.z = M_PI / m_vec_vel_states.back().duration.toSec();
// 2. state: stop for 2 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(3.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.00;
m_vec_vel_states.back().cmd_vel.angular.z = 0.00;
// 3. state: drive anti-clockwise half circle witch 180 degree turn in 5 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(5.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.01;
m_vec_vel_states.back().cmd_vel.angular.z = M_PI / m_vec_vel_states.back().duration.toSec();
// 4. state: stop for 2 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(3.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.00;
m_vec_vel_states.back().cmd_vel.angular.z = 0.00;
// 5. state: drive clockwise half circle witch 180 degree turn in 5 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(5.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.01;
m_vec_vel_states.back().cmd_vel.angular.z = -M_PI / m_vec_vel_states.back().duration.toSec();
// 6. state: stop for 2 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(3.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.00;
m_vec_vel_states.back().cmd_vel.angular.z = 0.00;
// 7. state: drive clockwise half circle witch 180 degree turn in 5 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(5.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.01;
m_vec_vel_states.back().cmd_vel.angular.z = -M_PI / m_vec_vel_states.back().duration.toSec();
// 8. state: stop for 2 seconds
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(3.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.00;
m_vec_vel_states.back().cmd_vel.angular.z = 0.00;
}
/*
* Destructor
*/
sick_line_guidance_demo::FigureEightVelocityFSM::~FigureEightVelocityFSM()
{
}
/*
* Next cycle, internal state is updated, velocity message may switch to next movement
*/
void sick_line_guidance_demo::FigureEightVelocityFSM::update(void)
{
ros::Time cur_time = ros::Time::now();
if(!m_next_state_switch.isValid())
{
m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration;
}
if(cur_time >= m_next_state_switch)
{
m_state_cnt = ((m_state_cnt + 1) % (m_vec_vel_states.size()));
m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration;
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "FigureEightVelocityFSM: state switched, cmd_vel: linear.x="
<< std::fixed << std::setprecision(3) << (m_vec_vel_states[m_state_cnt].cmd_vel.linear.x) << ", angular.z=" << (m_vec_vel_states[m_state_cnt].cmd_vel.angular.z/M_PI) << "*PI");
}
}
/*
* Returns the cmd_vel message for the current movement
*/
geometry_msgs::Twist sick_line_guidance_demo::FigureEightVelocityFSM::getVelocity(void)
{
return m_vec_vel_states[m_state_cnt].cmd_vel;
}

362
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/follow_line_state.cpp Executable file
View File

@@ -0,0 +1,362 @@
/*
* FollowLineState implements the state to follow a line for sick_line_guidance_demo.
* As long as ols detects a line, cmd_vel messages are published to follow this line.
* Input: ols and odometry messages
* Output: cmd_vel 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_demo/adjust_heading.h"
#include "sick_line_guidance_demo/follow_line_state.h"
#include "sick_line_guidance_demo/navigation_util.h"
#include "sick_line_guidance_demo/pid.h"
#include "sick_line_guidance_demo/regression_1d.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* Constructor
* @param[in] nh ros handle
* @param[in] context shared fsm context
*/
sick_line_guidance_demo::FollowLineState::FollowLineState(ros::NodeHandle* nh, RobotFSMContext* context) : m_fsm_context(context), m_last_barcode_detected_time(0), m_pid_kp(0), m_pid_ki(0), m_pid_kd(0), m_followLineRate(20)
{
if(nh)
{
// Configuration of PID controller
ros::param::getCached("/pid_controller/kp", m_pid_kp);
ros::param::getCached("/pid_controller/ki", m_pid_ki);
ros::param::getCached("/pid_controller/kd", m_pid_kd);
ros::param::getCached("/pid_controller/setpoint", m_pid_setpoint);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration pid_controller: kp=" << m_pid_kp << ", ki=" << m_pid_ki << ", kd=" << m_pid_kd << ", setpoint=" << m_pid_setpoint);
// Configuration of follow line state
double followLineRate;
ros::param::getCached("/follow_line_state/followLineRate", followLineRate); // frequency to update follow line state, default: 20 Hz
ros::param::getCached("/follow_line_state/followSpeed", m_followSpeed); // default linear velocity to follow a line
ros::param::getCached("/follow_line_state/noLineTime", m_noLineTime); // time in seconds before switching to state explore line because of lost line
ros::param::getCached("/follow_line_state/sensorLineWidth", m_sensorLineWidth); // measured line width at 0 degree, 29 mm for an OLS mounted 65 mm over ground, 20 mm for an OLS mounted 100 mm over ground
ros::param::getCached("/follow_line_state/sensorLineMeasurementJitter", m_sensorLineMeasurementJitter); // tolerate some line measurement jitter when adjusting the heading
ros::param::getCached("/follow_line_state/adjustHeadingAngularZ", m_adjustHeadingAngularZ); // velocity.angular.z to adjust robots heading
ros::param::getCached("/follow_line_state/adjustHeadingLcpDeviationThresh", m_adjustHeadingLcpDeviationThresh); // start to adjust heading, if the line distance increases over time (deviation of 1D-regression of line center points is above threshold lcpDeviationThresh)
ros::param::getCached("/follow_line_state/adjustHeadingLcpThresh", m_adjustHeadingLcpThresh); // start to adjust heading, if the line distance in meter (abs value) is above this threshold
ros::param::getCached("/follow_line_state/adjustHeadingDeltaAngleEpsilon", m_adjustHeadingDeltaAngleEpsilon); // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon
ros::param::getCached("/follow_line_state/adjustHeadingMinDistanceToLastAdjust", m_adjustHeadingMinDistanceToLastAdjust); // move at least some cm before doing next heading adjustment
ros::param::getCached("/follow_line_state/olsMessageTimeout", m_olsMessageTimeout); // timeout for ols messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
ros::param::getCached("/follow_line_state/odomMessageTimeout", m_odomMessageTimeout); // timeout for odom messages: robot stops and waits, if last ols message was received more than <timeout> seconds ago
ros::param::getCached("/wait_at_barcode_state/stopWaitSeconds", m_seconds_at_barcode); // time in seconds to wait at barcode
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration follow line state: "
<< " followLineRate=" << followLineRate << " followSpeed=" << m_followSpeed << ", noLineTime=" << m_noLineTime << ", sensorLineWidth=" << m_sensorLineWidth
<< ", sensorLineMeasurementJitter=" << m_sensorLineMeasurementJitter << ", adjustHeadingAngularZ=" << m_adjustHeadingAngularZ
<< ", adjustHeadingLcpDeviationThresh=" << m_adjustHeadingLcpDeviationThresh << ", adjustHeadingLcpThresh=" << m_adjustHeadingLcpThresh
<< ", adjustHeadingDeltaAngleEpsilon=" << m_adjustHeadingDeltaAngleEpsilon << ", adjustHeadingMinDistanceToLastAdjust=" << m_adjustHeadingMinDistanceToLastAdjust
<< ", olsMessageTimeout=" << m_olsMessageTimeout << ", odomMessageTimeout=" << m_odomMessageTimeout);
ros::Rate m_followLineRate = ros::Rate(followLineRate); // frequency to update follow line state, default: 20 Hz
ros::param::getCached("/sick_line_guidance_demo/ols_simu", m_ols_simu); // ols simulation (default: 0), test only
}
}
/*
* Destructor
*/
sick_line_guidance_demo::FollowLineState::~FollowLineState()
{
m_fsm_context = 0;
}
/*
* Clears all internal states (pid etc.)
*/
void sick_line_guidance_demo::FollowLineState::clear(void)
{
}
/*
* Runs the follow line state until line is lost (or a fatal error occures)
* @return EXPLORE_LINE in case of line lost, or EXIT in case ros::ok()==false.
*/
sick_line_guidance_demo::RobotFSMContext::RobotState sick_line_guidance_demo::FollowLineState::run(void)
{
assert(m_fsm_context);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: entering FollowLineState");
sick_line_guidance_demo::Regression1D lcp_regression(10); // regression over the last 10 line center points
sick_line_guidance_demo::AdjustHeading adjust_heading1; // state machine to adjust the heading, if the line distance increases over time.
sick_line_guidance_demo::AdjustHeading adjust_heading2; // state machine to adjust the heading, if the line width is out of plausible range.
size_t adjust_heading_cnt = 0; // count the number of heading adjustments required
float ols_msg_position = 0;
float ols_msg_linewidth = 0;
int follow_line_index = 1; // default: follow line with index 1, which is the main (center) line
int last_line_index = 1; // last line index 1, used to detect line switches at junctions and at turnouts
bool line_switched = false; // true if line switched at junctions and at turnouts (left or right line)
double typical_line_with = m_sensorLineWidth; // line width after heading adjustment
sick_line_guidance_demo::RobotPosition pos_at_last_adjustment(-FLT_MAX, -FLT_MAX, 0);
double pid_dt = m_followLineRate.expectedCycleTime().toSec();
PID_Controller pid(pid_dt, m_pid_kp, m_pid_ki, m_pid_kd);
double controller_value = 0; // pid-controller output
double followSpeed_in = 0;
bool line_detected = false;
ros::Time begin = ros::Time::now();
geometry_msgs::Twist velocityMessage;
velocityMessage.linear.x = 0;
velocityMessage.linear.y = 0;
velocityMessage.linear.z = 0;
velocityMessage.angular.x = 0;
velocityMessage.angular.y = 0;
velocityMessage.angular.z = 0;
while(ros::ok())
{
m_followLineRate.sleep();
// get parameters from ROS-parameter-server and configure the pid-controller
pid.setParams(pid_dt, m_pid_kp, m_pid_ki, m_pid_kd);
/* this is for slow acceleration */
if(followSpeed_in < m_followSpeed)
{
followSpeed_in += m_followSpeed / 30.0;
}
else
{
followSpeed_in = m_followSpeed;
}
// if no line detected, wait a time before raise /LOST_LINE, to get to Emergency_Stop state
sick_line_guidance::OLS_Measurement ols_msg = m_fsm_context->getOlsState();
sick_line_guidance_demo::RobotPosition odom_position = m_fsm_context->getOdomPosition();
bool ols_message_missing = m_fsm_context->hasOlsMessageTimeout(m_olsMessageTimeout);
bool odom_message_missing = m_fsm_context->hasOdomMessageTimeout(m_odomMessageTimeout);
if(ols_message_missing)
ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no ols message received since " << ((ros::Time::now() - m_fsm_context->getOlsStateTime()).toSec()) << " seconds");
if(odom_message_missing)
ROS_WARN_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: no odom message received since " << ((ros::Time::now() - m_fsm_context->getOdomPositionTime()).toSec()) << " seconds");
if(ols_message_missing || odom_message_missing)
{
velocityMessage.angular.z = 0;
velocityMessage.linear.x = 0;
m_fsm_context->publish(velocityMessage);
lcp_regression.clear();
pid.clear_state();
followSpeed_in = 0;
continue;
}
if(!sick_line_guidance_demo::NavigationUtil::lineDetected(ols_msg))
{
if(line_detected)
{
line_detected = false;
// save the last time, a line was detected
begin = ros::Time::now();
}
if(!adjust_heading1.isRunning() &&!adjust_heading2.isRunning() && (ros::Time::now() - begin).toSec() >= m_noLineTime)
{
// if a line is not detected for noLineTime, raise LOST_LINE to get in EmergencyStop state
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "LOST LINE -> switch to EXPLORE_LINE");
return RobotFSMContext::RobotState::EXPLORE_LINE;
}
}
else // if a line is detected, control the line guidance
{
line_detected = true;
}
bool barcode_detected = sick_line_guidance_demo::NavigationUtil::barcodeDetected(ols_msg);
if(!adjust_heading1.isRunning() && !adjust_heading2.isRunning())
{
// Switch to state WAIT_AT_BARCODE, if barcode detected and not currently adjusting
// uint32_t barcode = sick_line_guidance_demo::NavigationUtil::barcode(ols_msg);
if(barcode_detected && (ros::Time::now() - m_last_barcode_detected_time).toSec() > m_seconds_at_barcode+2) // time in seconds to wait at barcode
{
// new barcode detected -> switch to state WAIT_AT_BARCODE
m_last_barcode_detected_time = ros::Time::now();
return RobotFSMContext::RobotState::WAIT_AT_BARCODE;
}
if(barcode_detected)
m_last_barcode_detected_time = ros::Time::now();
}
if(line_detected)
{
follow_line_index = 1; // default: follow the center line
bool follow_left_turnout = m_fsm_context->getFollowLeftTurnout();
uint32_t last_barcode = m_fsm_context->getLastBarcode();
// Bit coding of parallel lines (ols_msg.status & 0x7):
// 2 = 2 : only one (center) line
// 4 | 2 = 6: right line and center line
/// 2 | 1 = 3: left line and center line
// Interpretation gives follow_line_index
// 0: left
// 1: center
// 2: right
if((ols_msg.status & 0x7) == 3 && !barcode_detected && follow_left_turnout && last_barcode == 101) // turnout to the left side detected, follow_left_turnout == true: follow a turnout on the left side, follow_left_turnout flag is toggled
{
follow_line_index = 0; // follow the left trace
}
if((ols_msg.status&0x7) == 6 && !barcode_detected && (!follow_left_turnout || last_barcode == 102)) // the outer (main) line is joining from the right, follow the outer one
{
follow_line_index = 2; // follow the right trace
}
ols_msg_position = ols_msg.position[follow_line_index];
ols_msg_linewidth = ols_msg.width[follow_line_index];
line_switched = (follow_line_index != last_line_index);
// if you have to change the line forget your regression history
if(line_switched)
{
lcp_regression.clear(); // line regression with lcp from different lines doesn't make sense
}
else
{
// smTimer sm_timer_lcp("lcp_regression");
lcp_regression.update(ols_msg_position); // update line regression by current lcp
}
if((ols_msg.status & 0x7) != 2) // junction
{
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << ((adjust_heading1.isRunning() || adjust_heading2.isRunning()) ? "ADJUST HEADING" : "FOLLOW LINE")
<< " ols_msg.status=" << (int)ols_msg.status << ", follow_left_turnout=" << follow_left_turnout << ", last_barcode=" << last_barcode << ", follow_line_index=" << follow_line_index);
}
}
// Adjust the robot heading, if the line distance increases over time. AdjustHeading searches the line orientation by minimizing the line distance and adjusts the robot heading.
// If the line distance is small, estimating heading from the line distance is inaccurate. Therefore, we adjust the heading by minimizing the line distance if the robot is outside the line
// (line distance > 10 mm or physicalLineWidth/2), and by minimizing the line width when inside the line (line distance < 10 mm or physicalLineWidth/2).
// smTimer sm_timer_adjust_init("adjust_header_init");
sick_line_guidance_demo::AdjustHeadingConfig adjust_heading_cfg;
adjust_heading_cfg.search_lower_bound_linedistance = 0.0; // lower limit for line distance: if the current line distance is below search_lower_bound_linedistance, the search will revert direction
adjust_heading_cfg.search_upper_bound_linedistance = std::abs(ols_msg_position) + m_sensorLineMeasurementJitter; // upper limit for line distance: if the current line distance is above search_upper_bound_linedistance, the search will revert direction
adjust_heading_cfg.search_lower_bound_linewidth = m_sensorLineWidth; // lower limit for line width: if the current line width is below search_lower_bound_linewidth, the search will revert direction
adjust_heading_cfg.search_upper_bound_linewidth = std::abs(ols_msg_linewidth) + 2 * m_sensorLineMeasurementJitter; // upper limit for line width: if the current line width is above search_upper_bound_linewidth, the search will revert direction
adjust_heading_cfg.search_max_yaw_angle_delta = 80.0 * M_PI / 180.0; // upper limit for yaw angle: if abs. difference between current yaw_angle and yaw angle at start, the search will revert direction
adjust_heading_cfg.angular_z = (velocityMessage.angular.z > 0) ? -m_adjustHeadingAngularZ : m_adjustHeadingAngularZ; // velocity.angular.z under adjustment, default: 0.1 * M_PI / 4
adjust_heading_cfg.measurement_jitter = m_sensorLineMeasurementJitter; // tolerate some line measurement jitter when adjusting the heading, default: 0.003
adjust_heading_cfg.delta_angle_epsilon = m_adjustHeadingDeltaAngleEpsilon; // search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon
adjust_heading_cfg.sigma_linedistance = 1.0; // standard deviation of line distance, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2)
adjust_heading_cfg.sigma_linewidth = 1.0; // standard deviation of line width, used to compute feature = sqrt((linedistance/sigma_linedistance)^2+(linewidth/sigma_linelinewidth)^2)
adjust_heading_cfg.max_state_duration = 10; // test only: max time amount in seconds to stay in current search state; after <max_state_duration> seconds state increases. Prevents endless adjustment if TurtleBot is mounted fixed under test and odom always returns constant positions
bool adjust_too_close_at_last = sick_line_guidance_demo::NavigationUtil::euclideanDistance(odom_position.pos, pos_at_last_adjustment.pos) < m_adjustHeadingMinDistanceToLastAdjust; // move at least some cm before doing next heading adjustment
double lcp_deviation = lcp_regression.getDeviation(); // get slope of deviation line and adjust heading if slope of deviation is above threshold (i.e. lcp increases resp. decreases over time)
bool lcp_deviation_exceed = (ols_msg_position > m_sensorLineWidth/2 && lcp_deviation > m_adjustHeadingLcpDeviationThresh) || (ols_msg_position < -m_sensorLineWidth/2 && lcp_deviation < -m_adjustHeadingLcpDeviationThresh); // slope of deviation line above threshold
bool lcp_max_dist_exceed = (std::abs(ols_msg_position) > m_adjustHeadingLcpThresh); // start to adjust heading, if the line distance in meter (abs value) is above this threshold
bool adjust_after_line_switch = line_switched && (follow_line_index == 1); // adjust heading after a line switch is recommanded, heading can be quite different from the new line
// distance to line center is increasing over time -> rotate and minimize distance to line
bool enable_adjust_heading1 = (barcode_detected ? (std::abs(ols_msg_position) > m_sensorLineWidth/2 + m_sensorLineMeasurementJitter) : (std::abs(ols_msg_position) > m_sensorLineWidth - m_sensorLineMeasurementJitter));
// robot near line center and line width too high -> rotate and minimize line center and line width
bool enable_adjust_heading2 = std::abs(ols_msg_position) <= m_sensorLineWidth/2 + m_sensorLineMeasurementJitter && std::abs(ols_msg_linewidth) >= std::max(typical_line_with, m_sensorLineWidth) + m_sensorLineMeasurementJitter;
enable_adjust_heading2 |= adjust_after_line_switch; // tbd: testen ...
bool adjust_heading_line_detected = line_detected && (follow_line_index == 1); // When switching to another line at junctions, we wait to adjust heading until the line to follow is the center line
if(!adjust_heading1.isRunning() && !adjust_heading2.isRunning() && adjust_heading_line_detected && (lcp_deviation_exceed || lcp_max_dist_exceed) && !adjust_too_close_at_last && enable_adjust_heading1 && adjust_heading_cnt)
{
// line detected and line distance increases over time -> start adjustment of robots heading
adjust_heading1.start(ols_msg_position, ols_msg_linewidth, odom_position.yaw, false, adjust_heading_cfg);
}
else if(!adjust_heading1.isRunning() && !adjust_heading2.isRunning() && adjust_heading_line_detected && !barcode_detected && !adjust_too_close_at_last && enable_adjust_heading2) // line width not meaningsfull within barcode, do not use for adjust heading
{
// sensor position within the line, but probably not heading in line direction (line width measured > sensorLineWidth) -> start adjust heading
adjust_heading2.start(ols_msg_position, ols_msg_linewidth, odom_position.yaw, true, adjust_heading_cfg);
}
// sm_timer_adjust_init.stop();
// update line distance and width, if adjust heading is running, and a line is detected
// smTimer sm_timer_adjust_update("adjust_header_update");
double adjust_heading1_angular_z = adjust_heading1.update(adjust_heading_line_detected, ols_msg_position, ols_msg_linewidth, odom_position.yaw);
double adjust_heading2_angular_z = adjust_heading2.update(adjust_heading_line_detected && !barcode_detected, ols_msg_position, ols_msg_linewidth, odom_position.yaw);
if(adjust_heading1.isRunning() || adjust_heading2.isRunning())
{
adjust_heading_cnt++;
pos_at_last_adjustment = odom_position;
typical_line_with = (adjust_heading1.isRunning() ? adjust_heading1.getBestLineWidth() : adjust_heading2.getBestLineWidth());
if(m_ols_simu > 0) // test only (default: 0)
typical_line_with = m_sensorLineWidth;
velocityMessage.linear.x = 0;
velocityMessage.angular.z = (adjust_heading1.isRunning() ? adjust_heading1_angular_z : adjust_heading2_angular_z);
lcp_regression.clear();
pid.clear_state();
followSpeed_in = 0;
}
else // if(line_detected) // OLS detects no line when entering the barcode area. So we have to continue nevertheless we have detected a line or not.
{
if(line_detected) // ols_msg_position is valid, deviation = set value - actual value
controller_value = pid.calculate_output(m_pid_setpoint, ols_msg_position);
else
controller_value = 0; // ols_msg_position not valid, we continue straight forward
velocityMessage.angular.z = controller_value; // control angular velocity by pid controller
velocityMessage.linear.x = followSpeed_in; // set linear velocity
}
// sm_timer_adjust_update.stop();
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << ((adjust_heading1.isRunning() || adjust_heading2.isRunning()) ? "ADJUST HEADING" : "FOLLOW LINE")
<< ": detected=" << (int)line_detected << ", barcode=" << (int)barcode_detected << "(" << (int)ols_msg.barcode << "), linepos=" << ols_msg_position << ", linewidth=" << ols_msg_linewidth
<< std::fixed << std::setprecision(3) << ", velocity.linear.x=" << velocityMessage.linear.x << ", velocity.angular.z=" << velocityMessage.angular.z/M_PI << "*PI"
<< ", odom=(x:" << odom_position.pos.x << ",y:" << odom_position.pos.y << ",yaw:" << odom_position.yaw/M_PI << "*PI)");
// Hard limits for the velocity, both angular and linear, to prevent oscillation by PID:
// Incorrect robot heading increases the line distance, with causes the PID to compensate by making the heading even worse ...
// If abs(velocityMessage.angular.z) is too high, the robot may turn sharply and hit the line orthogonal, causing movements like a sinus wave with increasing amplitude.
// If abs(velocityMessage.linear.x) is too high, the robot may not be able to turn in time, i.e. before loosing the line.
// smTimer sm_timer_publish("publish_cmd_vel");
double max_cmd_vel_angular = 0.5 * 0.7854; // 0.7854 = PI/4
double max_cmd_vel_linear = 0.04;
// Force slow down if we're missing the center of the line
if(std::abs(ols_msg_position) > 0.003)
max_cmd_vel_linear = 0.03;
velocityMessage.angular.z = std::min(velocityMessage.angular.z, max_cmd_vel_angular);
velocityMessage.angular.z = std::max(velocityMessage.angular.z, -max_cmd_vel_angular);
velocityMessage.linear.x = std::min(velocityMessage.linear.x, max_cmd_vel_linear);
velocityMessage.linear.x = std::max(velocityMessage.linear.x, -max_cmd_vel_linear);
m_fsm_context->publish(velocityMessage); // publish controller value to cmd_vel topic
last_line_index = follow_line_index;
}
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: leaving FollowLineState");
return RobotFSMContext::RobotState::EXIT; // ros::ok() == false
}

197
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/image_util.cpp Executable file
View File

@@ -0,0 +1,197 @@
/*
* ImageUtil implements utility functions for images and maps.
*
* 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 <cmath>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include "sick_line_guidance_demo/image_util.h"
#include "sick_line_guidance_demo/navigation_util.h"
/*
* computes and returns the max possible euclidean distance of a robot position to the corner points of an image.
* @param[in] robot_pos robots position on the navigation map [pixel]
* @param[in] dimx width of navigation map [pixel]
* @param[in] dimy height of navigation map [pixel]
* @return max possible distance [pixel]
*/
double sick_line_guidance_demo::ImageUtil::computeMaxDistanceToCorner(const cv::Point & robot_pos, int dimx, int dimy)
{
double r1_sqr = (robot_pos.x - 0) * (robot_pos.x - 0) + (robot_pos.y - 0) * (robot_pos.y - 0); // square distance to upper left corner
double r2_sqr = (robot_pos.x - dimx) * (robot_pos.x - dimx) + (robot_pos.y - 0) * (robot_pos.y - 0); // square distance to upper right corner
double r3_sqr = (robot_pos.x - dimx) * (robot_pos.x - dimx) + (robot_pos.y - dimy) * (robot_pos.y - dimy); // square distance to lower right corner
double r4_sqr = (robot_pos.x - 0) * (robot_pos.x - 0) + (robot_pos.y - dimy) * (robot_pos.y - dimy); // square distance to lower left corner
double r_sqr = std::max(std::max(r1_sqr, r2_sqr), std::max(r3_sqr, r4_sqr)); // max square distance
return sqrt(r_sqr); // max distance
}
/*
* computes and returns the position of a point with a distance <radius> and in direction <heading> from a given point <base_pos>.
* @param[in] base_pos start position in world coordinates [meter]
* @param[in] heading angle between given point <base_pos> and the returned point
* @param[in] radius distance of the returned point to <base_pos>
* @return position of a point with a distance <radius> and in direction <heading> from a given point <base_pos> in world coordinates [meter]
*/
cv::Point2d sick_line_guidance_demo::ImageUtil::getWorldPointInDirection(const cv::Point2d & base_pos, double heading, double radius)
{
cv::Point2d delta(radius * cos(heading), radius * sin(heading));
return cv::Point2d(base_pos.x + delta.x, base_pos.y + delta.y);
}
/*
* computes and returns the position of a point with a distance <radius> and in direction <heading> from a given point <base_pos>.
* @param[in] base_pos start position in map coordinates [pixel]
* @param[in] heading angle between given point <base_pos> and the returned point
* @param[in] radius distance of the returned point to <base_pos>
* @return position of a point with a distance <radius> and in direction <heading> from a given point <base_pos> in map coordinates [pixel]
*/
cv::Point sick_line_guidance_demo::ImageUtil::getMapPointInDirection(const cv::Point & base_pos, double heading, double radius)
{
cv::Point2d delta(radius * cos(heading), radius * sin(heading));
return cv::Point(base_pos.x + std::lround(delta.x), base_pos.y - std::lround(delta.y));
}
/*
* Detects and returns the line center points on a map, which can be seen
* by a robot at position <robot_pos> moving in directions <robot_heading>.
* Lines are detected +- 90 degree of <robot_heading>.
* @param[in] map_img image (navigation map)
* @param[in] robot_map_pos robots position on the navigation map
* @param[in] robot_heading robots moving direction (i.e. robots yaw angle)
* @return list of line detection results (center points etc.) in map coordinates [pixel]
*/
std::vector<sick_line_guidance_demo::LineDetectionResult> sick_line_guidance_demo::ImageUtil::detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading)
{
// Get 2 starting points for cv::LineIterator in max. distance to robot_map_pos in orthogonal direction (+- 90 degree) to robot_heading
double r_max = sick_line_guidance_demo::ImageUtil::computeMaxDistanceToCorner(robot_map_pos, map_img.cols, map_img.rows);
cv::Point search_start_pos = getMapPointInDirection(robot_map_pos, robot_heading + CV_PI/2, r_max);
cv::Point search_end_pos = getMapPointInDirection(robot_map_pos, robot_heading - CV_PI/2, r_max);
cv::clipLine(cv::Size(map_img.cols, map_img.rows), search_start_pos, search_end_pos);
return detectLineCenterPoints(map_img, robot_map_pos, search_start_pos, search_end_pos);
}
/*
* Detects and returns the line center points on a map, in front of
* a robot at position <robot_map_pos> moving in directions <robot_heading>.
* Lines are detected in heading of <robot_heading>.
* @param[in] map_img image (navigation map)
* @param[in] robot_map_pos robots position on the navigation map
* @param[in] robot_heading robots moving direction (i.e. robots yaw angle)
* @return list of line detection results (center points etc.) in map coordinates [pixel]
*/
std::vector<sick_line_guidance_demo::LineDetectionResult> sick_line_guidance_demo::ImageUtil::detectLineCenterInHeadingDirection(const cv::Mat & map_img, const cv::Point & robot_map_pos, double robot_heading)
{
// Get 2 starting points for cv::LineIterator in max. distance to robot_map_pos in orthogonal direction (+- 90 degree) to robot_heading
double r_max = sick_line_guidance_demo::ImageUtil::computeMaxDistanceToCorner(robot_map_pos, map_img.cols, map_img.rows);
cv::Point search_start_pos = robot_map_pos;
cv::Point search_end_pos = getMapPointInDirection(robot_map_pos, robot_heading, r_max);
cv::clipLine(cv::Size(map_img.cols, map_img.rows), search_start_pos, search_end_pos);
return detectLineCenterPoints(map_img, robot_map_pos, robot_map_pos, search_end_pos);
}
/*
* detects and returns the line center points on a map, which can be seen
* by a robot at position <robot_map_pos> moving in directions <robot_heading>.
* @param[in] map_img image (navigation map)
* @param[in] robot_map_pos robots position on the navigation map
* @param[in] search_start_pos start point for line iteration
* @param[in] search_end_pos end point for line iteration
* @return list of line detection results (center points etc.) in map coordinates [pixel]
*/
std::vector<sick_line_guidance_demo::LineDetectionResult> sick_line_guidance_demo::ImageUtil::detectLineCenterPoints(const cv::Mat & map_img, const cv::Point & robot_map_pos, const cv::Point2d & search_start_pos, const cv::Point2d & search_end_pos)
{
// Iterate from search start to end position and detect the line starts (pixel changes from not-black to black) and the line ends (pixel changes from black to not-black)
std::vector<sick_line_guidance_demo::LineDetectionResult> vec_line_detection_result;
bool lastPixelWasLine = false;
cv::Point2d robot_pos2d(robot_map_pos.x, robot_map_pos.y);
cv::Point2d line_start_pos(0, 0);
cv::Point2d line_end_pos(0, 0);
cv::LineIterator search_iter(map_img, search_start_pos, search_end_pos);
for(int cnt_iter = 0; cnt_iter < search_iter.count; cnt_iter++, search_iter++)
{
bool isLinePixel = ImageUtil::isLinePixel(map_img, search_iter.pos());
if(isLinePixel) // line detected
{
line_end_pos = cv::Point2d(search_iter.pos().x, search_iter.pos().y);
}
if(!lastPixelWasLine && isLinePixel) // start of a line detected
{
line_start_pos = cv::Point2d(search_iter.pos().x, search_iter.pos().y);
}
else if(lastPixelWasLine && !isLinePixel && line_start_pos != line_end_pos) // end of a line detected, append line center point to output result (1 pixel lines ignored)
{
double line_center_posx = 0.5 * (line_start_pos.x + line_end_pos.x);
double line_center_posy = 0.5 * (line_start_pos.y + line_end_pos.y);
cv::Point2d line_center (line_center_posx, line_center_posy);
double line_width = NavigationUtil::euclideanDistance(line_start_pos, line_end_pos) + 1;
double center_dist = NavigationUtil::euclideanDistance(line_center, robot_pos2d);
LineDetectionResult result(line_center, line_start_pos, line_end_pos, line_width, center_dist);
vec_line_detection_result.push_back(result);
}
lastPixelWasLine = isLinePixel;
}
return vec_line_detection_result;
}

494
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_mapper.cpp Executable file
View File

@@ -0,0 +1,494 @@
/*
* NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position,
* detect lines and barcodes in the map plus their distance to the robot,
* and transforms them invers into world coordinates.
*
* 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/thread.hpp>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
#include "sick_line_guidance_demo/image_util.h"
#include "sick_line_guidance_demo/navigation_mapper.h"
#include "sick_line_guidance_demo/navigation_util.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* class NavigationMapper transforms the robots xy-positions from world/meter into map/pixel position,
* detect lines and barcodes in the map plus their distance to the robot,
* and transforms them invers into world coordinates.
*/
/*
* Constructor
* @param[in] map_imagefile navigation map, image file containing the map, f.e. "demo_map_02.png"
* @param[in] intrinsic_xmlfile xmlfile with intrinsic parameter to transform position from navigation map (pixel) to real world (meter) and vice versa, f.e. "cam_intrinsic.xml" with cx=cy=660, fx=fy=1
* @param[in] barcode_xmlfile xmlfile with a list of barcodes with label and position, f.e. "demo_barcodes.xml"
* @param[in] ros_topic_output_ols_messages ROS topic for simulated OLS_Measurement messages, "/ols" (activated) in simulation and "" (deactivated) in demo application
* @param[in] sensor_config line sensor configuration setting (sensor parameter and mounting settings)
* @param[in] visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled.
*/
sick_line_guidance_demo::NavigationMapper::NavigationMapper(ros::NodeHandle* nh, const std::string & map_imagefile, const std::string & intrinsic_xmlfile, const std::string & barcode_xmlfile, const std::string & ros_topic_output_ols_messages,
const LineSensorConfig & sensor_config, int visualize)
: m_navigation_state(INITIAL), m_visualize(visualize), m_window_name(""), m_ols_measurement_simulator(nh, ros_topic_output_ols_messages), m_sensor_config(sensor_config), m_message_rate(20), m_message_thread_run(false), m_message_thread(0)
{
sick_line_guidance::OLS_Measurement zero_ols_msg;
sick_line_guidance::MsgUtil::zero(zero_ols_msg);
m_ols_measurement_sensor.set(zero_ols_msg);
m_ols_measurement_world.set(zero_ols_msg);
m_error_simulation_burst_no_line_duration = 0.0;
m_error_simulation_burst_no_line_frequency = 0.0;
m_error_simulation_no_line_start = ros::Time::now();
m_error_simulation_no_line_end = m_error_simulation_no_line_start;
if(nh)
{
ros::param::getCached("/error_simulation/burst_no_line_duration", m_error_simulation_burst_no_line_duration); // error simulation: duration of "no line detected" bursts in seconds, default: 0.0 (disabled)
ros::param::getCached("/error_simulation/burst_no_line_frequency", m_error_simulation_burst_no_line_frequency); // error simulation: frequency of "no line detected" bursts in 1/seconds, default: 0.0 (disabled)
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration error simulation: "
<< " m_error_simulation_burst_no_line_duration=" << m_error_simulation_burst_no_line_duration
<< " m_error_simulation_burst_no_line_frequency=" << m_error_simulation_burst_no_line_frequency);
}
m_message_rate = ros::Rate(20);
if(!map_imagefile.empty())
{
m_map_img = cv::imread(map_imagefile, cv::IMREAD_COLOR);
if(m_map_img.empty())
{
ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::NavigationMapper(): file \"" << map_imagefile << "\" not readable (" << __FILE__ << ":" << __LINE__ << ").");
m_visualize = false;
}
}
if(!intrinsic_xmlfile.empty())
{
cv::FileStorage cv_intrinsics_file(intrinsic_xmlfile, cv::FileStorage::READ);
cv_intrinsics_file["Camera_Matrix"] >> m_intrinsics;
cv_intrinsics_file.release();
if(m_intrinsics.empty())
{
ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::NavigationMapper(): intrisic matrix file \"" << intrinsic_xmlfile << "\" not readable or parameter not found (" << __FILE__ << ":" << __LINE__ << ").");
}
else
{
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: intrinsics {cx=" << m_intrinsics.at<double>(0,2) << ",cy=" << m_intrinsics.at<double>(1,2) << ",fx=" << m_intrinsics.at<double>(0,0) << ",fy=" << m_intrinsics.at<double>(1,1) << "} read from configuration file \"" << intrinsic_xmlfile << "\".");
m_intrinsics_inv = m_intrinsics.inv();
}
}
if(!barcode_xmlfile.empty())
{
m_barcodes = BarcodeUtil::readBarcodeXmlfile(barcode_xmlfile);
if(m_barcodes.empty())
{
ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::NavigationMapper(): barcodes configuration file \"" << barcode_xmlfile << "\" not readable or parameter not found (" << __FILE__ << ":" << __LINE__ << ").");
}
else
{
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: " << m_barcodes.size() << " barcodes read from configuration file \"" << barcode_xmlfile << "\".");
for(std::vector<Barcode>::iterator iter_barcode = m_barcodes.begin(); iter_barcode != m_barcodes.end(); iter_barcode++)
{
iter_barcode->innerRectMap() = transformRectWorldToMap(iter_barcode->innerRectWorld());
iter_barcode->outerRectMap() = transformRectWorldToMap(iter_barcode->outerRectWorld());
ROS_DEBUG_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "barcode: label=\"" << iter_barcode->label() << "\", innerRect=" << iter_barcode->innerRectWorld()<< "\", outerRect=" << iter_barcode->outerRectWorld() << ", flipped=" << iter_barcode->flipped());
}
}
}
if(m_visualize > 0)
{
m_window_img = m_map_img.clone();
m_video_write = cv::VideoWriter("sick_line_guidance_demo.avi",CV_FOURCC('F','M','P','4'), 30, cv::Size(m_window_img.cols, m_window_img.rows));
if(m_visualize > 1)
{
m_window_name = "NavigationMap";
cv::namedWindow(m_window_name, cv::WINDOW_AUTOSIZE);
cv::imshow(m_window_name, m_window_img);
}
}
}
/*
* Destructor
*/
sick_line_guidance_demo::NavigationMapper::~NavigationMapper()
{
if(m_visualize > 0)
{
m_video_write.release();
if(m_visualize > 1)
{
cv::destroyAllWindows();
}
}
}
/*
* transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel]
* @param[in] world_pos xy-position in world coordinates [meter]
* @param[out] map_pos xy-position in image map coordinates [pixel]
*/
void sick_line_guidance_demo::NavigationMapper::transformPositionWorldToMap(const cv::Point2d & world_pos, cv::Point2d & map_pos)
{
double world_vec[3] = { world_pos.x, world_pos.y, 1.0 };
cv::Mat world_mat = cv::Mat(3, 1, CV_64F, world_vec);
cv::Mat map_mat = m_intrinsics * world_mat;
map_pos.x = map_mat.at<double>(0,0);
map_pos.y = map_mat.at<double>(1,0);
}
/*
* transforms a xy-position in world coordinates [meter] to a xy-position in image map coordinates [pixel]
* @param[in] world_pos xy-position in world coordinates [meter]
* @return xy-position in image map coordinates [pixel]
*/
cv::Point sick_line_guidance_demo::NavigationMapper::transformPositionWorldToMap(const cv::Point2d & world_pos)
{
cv::Point2d map_pos2d;
transformPositionWorldToMap(world_pos, map_pos2d);
return cv::Point(std::lround(map_pos2d.x), std::lround(map_pos2d.y));
}
/*
* transforms a xy-position in image map coordinates [pixel] to a xy-position in world coordinates [meter]
* @param[in] map_pos xy-position in image map coordinates [pixel]
* @return xy-position in world coordinates [meter]
*/
cv::Point2d sick_line_guidance_demo::NavigationMapper::transformPositionMapToWorld(const cv::Point2d & map_pos)
{
double map_vec[3] = { map_pos.x, map_pos.y, 1.0 };
cv::Mat map_mat = cv::Mat(3, 1, CV_64F, map_vec);
cv::Mat world_mat = m_intrinsics_inv * map_mat;
return cv::Point2d(world_mat.at<double>(0,0), world_mat.at<double>(1,0));
}
/*
* transforms a rectangle in world coordinates [meter] into a rectangle in image map coordinates [pixel]
* @param[in] world_rect rectangle in world coordinates [meter]
* @return rectangle in image map coordinates [pixel]
*/
cv::Rect sick_line_guidance_demo::NavigationMapper::transformRectWorldToMap(const cv::Rect2d & world_rect)
{
cv::Point map_pos1 = transformPositionWorldToMap(cv::Point2d(world_rect.x, world_rect.y));
cv::Point map_pos2 = transformPositionWorldToMap(cv::Point2d(world_rect.x + world_rect.width, world_rect.y));
cv::Point map_pos3 = transformPositionWorldToMap(cv::Point2d(world_rect.x + world_rect.width, world_rect.y + world_rect.height));
cv::Point map_pos4 = transformPositionWorldToMap(cv::Point2d(world_rect.x, world_rect.y + world_rect.height));
int x1 = std::min(std::min(map_pos1.x, map_pos2.x), std::min(map_pos3.x, map_pos4.x));
int y1 = std::min(std::min(map_pos1.y, map_pos2.y), std::min(map_pos3.y, map_pos4.y));
int x2 = std::max(std::max(map_pos1.x, map_pos2.x), std::max(map_pos3.x, map_pos4.x));
int y2 = std::max(std::max(map_pos1.y, map_pos2.y), std::max(map_pos3.y, map_pos4.y));
return cv::Rect(x1, y1, x2 - x1 + 1, y2 - y1 + 1);
}
/*
* transforms an ols state from world coordinates [meter] to sensor units [meter], i.e. scales line distance (lcp) and line width
* from physical distances in the world to units of a sensor measurement; reverts function unscaleMeasurementToWorld():
* line distances (lcp) are scaled by line_sensor_scaling_dist: default: 180.0/133.0, Scaling between physical distance to the line center and measured line center point
* (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height
* line width are scaled by line_sensor_scaling_width: default: 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)
* @param[in] ols_state ols state in world coordinates [meter]
* @return ols measurement in sensor units [meter]
*/
sick_line_guidance::OLS_Measurement sick_line_guidance_demo::NavigationMapper::scaleWorldToMeasurement(const sick_line_guidance::OLS_Measurement & ols_state)
{
sick_line_guidance::OLS_Measurement ols_msg = ols_state;
int status_flags[3] = { 0x1, 0x2, 0x4 };
for(int line_idx = 0; line_idx < 3; line_idx++)
{
if((ols_msg.status & (status_flags[line_idx])) != 0)
{
ols_msg.position[line_idx] *= static_cast<float>(m_sensor_config.line_sensor_scaling_dist);
ols_msg.width[line_idx] *= static_cast<float>(m_sensor_config.line_sensor_scaling_width);
}
}
return ols_msg;
}
/*
* transforms an ols measurement from sensor units [meter] to world coordinates [meter], i.e. scales line distance (lcp) and line width
* from sensor units to physical distances; reverts function scaleWorldToMeasurement().
* line distances (lcp) are scaled by 1 / line_sensor_scaling_dist
* line width are scaled by 1 / line_sensor_scaling_width
* @param[in] ols measurement in sensor units [meter]
* @return ols_state ols state in world coordinates [meter]
*/
sick_line_guidance::OLS_Measurement sick_line_guidance_demo::NavigationMapper::unscaleMeasurementToWorld(const sick_line_guidance::OLS_Measurement & ols_message)
{
sick_line_guidance::OLS_Measurement ols_state = ols_message;
int status_flags[3] = { 0x1, 0x2, 0x4 };
for(int line_idx = 0; line_idx < 3; line_idx++)
{
if((ols_state.status & (status_flags[line_idx])) != 0)
{
ols_state.position[line_idx] /= static_cast<float>(m_sensor_config.line_sensor_scaling_dist);
ols_state.width[line_idx] /= static_cast<float>(m_sensor_config.line_sensor_scaling_width);
}
}
return ols_state;
}
/*
* starts the message loop to handle ols and odometry messages received.
* Message handling runs in background thread started by this function.
*/
void sick_line_guidance_demo::NavigationMapper::start(void)
{
stop();
m_message_thread_run = true;
m_message_thread = new boost::thread(&sick_line_guidance_demo::NavigationMapper::messageLoop, this);
}
/*
* stops the message loop.
*/
void sick_line_guidance_demo::NavigationMapper::stop(void)
{
if(m_message_thread)
{
m_message_thread_run = false;
m_message_thread->join();
delete(m_message_thread);
m_message_thread = 0;
}
}
/*
* message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols".
* It displays the OLS measurement (line info and barcodes), if visualization is enabled.
* @param[in] msg OLS measurement message (input)
*/
void sick_line_guidance_demo::NavigationMapper::messageCallbackOlsMeasurement(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg)
{
if(msg)
{
m_ols_measurement_sensor.set(*msg); // OLS measurement message received from topic "/ols" (measurment in sensor units)
m_ols_measurement_world.set(unscaleMeasurementToWorld(*msg)); // OLS measurement message received from topic "/ols" (measurment scaled world coordinates)
}
else
{
ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::messageCallbackOlsMeasurement(): invalid message (" << __FILE__ << ":" << __LINE__ << ")");
}
}
/*
* message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom".
* @param[in] msg odometry message (input)
*/
void sick_line_guidance_demo::NavigationMapper::messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
{
if(msg)
{
m_odom_msg.set(*msg);
}
else
{
ROS_ERROR_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "## ERROR sick_line_guidance_demo::NavigationMapper::messageCallbackOdometry(): invalid message (" << __FILE__ << ":" << __LINE__ << ")");
}
}
/*
* Runs the message loop to handle odometry and ols messages.
* It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot,
* and transform them invers into world coordinates.
* @param[in] msg odometry message (input)
*/
void sick_line_guidance_demo::NavigationMapper::messageLoop(void)
{
while(ros::ok())
{
nav_msgs::Odometry odom_msg = m_odom_msg.get();
// Convert ModelStates message to robot position in world coordinates [meter] and and image position [pixel]
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper::messageLoop: ols_msg_sensor=( " << sick_line_guidance::MsgUtil::toInfo(m_ols_measurement_sensor.get()) << " )");
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper::messageLoop: ols_msg_world=( " << sick_line_guidance::MsgUtil::toInfo(m_ols_measurement_world.get()) << " )");
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper::messageLoop: odom_msg=( " << NavigationUtil::toInfo(odom_msg) << " )");
double robot_world_posx = 0, robot_world_posy= 0, robot_yaw_angle = 0;
NavigationUtil::toWorldPosition(odom_msg, robot_world_posx, robot_world_posy, robot_yaw_angle);
cv::Point2d robot_world_pos(robot_world_posx, robot_world_posy);
cv::Point robot_map_pos = transformPositionWorldToMap(robot_world_pos);
sick_line_guidance::OLS_Measurement ols_state = unscaleMeasurementToWorld(m_ols_measurement_simulator.GetState());
// start quickfix to find a line initially (simulation only): force an initial turn to the left to hit the line for the first time, needs further handling - tbd ...
if(m_navigation_state == INITIAL && (robot_world_pos.x*robot_world_pos.x+robot_world_pos.y*robot_world_pos.y) > (0.26*0.26)) // force initial turn to the left to hit the line for the first time
{
OLS_Measurement_Simulator::setLine(ols_state, 0.001f, 0.02); // force a slight left turn
}
if(m_navigation_state == INITIAL && ImageUtil::isLinePixel(m_map_img,robot_map_pos))
m_navigation_state = FOLLOW_LINE; // line close to sensor, start to follow this line
// end quicktest
// Detect possible line center points in map/image coordinates [pixel]
std::vector<LineDetectionResult> map_line_points = ImageUtil::detectLineCenterPoints(m_map_img, robot_map_pos, robot_yaw_angle);
// transform line center points to world coordinate and compute distance in meter
std::vector<LineDetectionResult> world_line_points;
for(std::vector<LineDetectionResult>::iterator iter_line_points = map_line_points.begin(); iter_line_points != map_line_points.end(); iter_line_points++)
{
LineDetectionResult world_line_pos(*iter_line_points);
world_line_pos.centerPos() = transformPositionMapToWorld(iter_line_points->centerPos());
world_line_pos.startPos() = transformPositionMapToWorld(iter_line_points->startPos());
world_line_pos.endPos() = transformPositionMapToWorld(iter_line_points->endPos());
world_line_pos.lineWidth() = NavigationUtil::euclideanDistance(world_line_pos.startPos(), world_line_pos.endPos());
world_line_pos.centerDistance() = NavigationUtil::euclideanDistanceOrientated(robot_world_pos, world_line_pos.centerPos(), robot_yaw_angle, m_sensor_config.line_sensor_mounted_right_to_left);
world_line_points.push_back(world_line_pos);
}
// Sort by ascending distance between robot and line center point (note: centerDistance() is orientated, use std::abs(centerDistance()) for comparison)
std::sort(world_line_points.begin(), world_line_points.end(), [](const LineDetectionResult & a, const LineDetectionResult & b){ return (std::abs(a.centerDistance()) < std::abs(b.centerDistance())); });
// Get a list of line center points within the detection zone of the sensor
Barcode barcode;
std::vector<LineDetectionResult> sensor_line_points = NavigationUtil::selectLinePointsWithinDetectionZone(world_line_points, robot_world_pos, m_sensor_config.line_sensor_detection_width, 3);
if(m_navigation_state > INITIAL)
{
// Set ols measurement from line center points within the detection zone of the sensor
for(std::vector<LineDetectionResult>::iterator iter_line_points = sensor_line_points.begin(); iter_line_points != sensor_line_points.end(); iter_line_points++)
{
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "OLS-Simulation: robot_world_pos=(" << std::setprecision(3) << std::fixed << robot_world_pos.x << "," << robot_world_pos.y << "), lcp_pos=(" << iter_line_points->centerPos().x << "," << iter_line_points->centerPos().y << "), lcp_dist=" << iter_line_points->centerDistance() << "), linewidth=" << iter_line_points->lineWidth());
}
// Detect barcodes
for(std::vector<Barcode>::iterator iter_barcodes = m_barcodes.begin(); barcode.label().empty() && iter_barcodes != m_barcodes.end(); iter_barcodes++)
{
if(iter_barcodes->innerRectWorld().contains(robot_world_pos))
{
barcode = *iter_barcodes;
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: Barcode \"" << barcode.label() << "\" detected at robot xy-position on world: (" << std::setprecision(3) << std::fixed << robot_world_pos.x << "," << robot_world_pos.y << ")");
for(std::vector<LineDetectionResult>::iterator iter_line_points = sensor_line_points.begin(); iter_line_points != sensor_line_points.end(); iter_line_points++)
iter_line_points->lineWidth() = barcode.innerRectWorld().width; // Line width within label area of the barcode has max. size
}
}
OLS_Measurement_Simulator::setLines(ols_state, sensor_line_points);
OLS_Measurement_Simulator::setBarcode(ols_state, barcode.labelCode(), barcode.flipped());
}
// Error simulation and testing: no line detected for some time (line damaged or barcode entered) => fsm must not hang or loose the track!
if(m_navigation_state > INITIAL && m_error_simulation_burst_no_line_frequency > 0 && m_error_simulation_burst_no_line_duration > 0
&& ros::Time::now() > m_error_simulation_no_line_end + ros::Duration(1/m_error_simulation_burst_no_line_frequency))
{
m_error_simulation_no_line_start = ros::Time::now();
m_error_simulation_no_line_end = m_error_simulation_no_line_start + ros::Duration(m_error_simulation_burst_no_line_duration);
}
if(ros::Time::now() >= m_error_simulation_no_line_start && ros::Time::now() < m_error_simulation_no_line_end)
{
sensor_line_points.clear();
OLS_Measurement_Simulator::setLines(ols_state, sensor_line_points);
}
// Publish ols measurement (if we're running the simulation)
sick_line_guidance::OLS_Measurement ols_msg = scaleWorldToMeasurement(ols_state);
m_ols_measurement_simulator.SetState(ols_msg);
if(!m_ols_measurement_simulator.getPublishTopic().empty())
{
m_ols_measurement_sensor.set(ols_msg);
m_ols_measurement_world.set(ols_state);
m_ols_measurement_simulator.schedulePublish();
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: publishing lcp[1]=" << std::fixed << std::setprecision(3) << ols_msg.position[1]
<< ", width[1]=" << ols_msg.width[1] << " on topic \"" << m_ols_measurement_simulator.getPublishTopic() << "\"");
}
if(m_visualize)
{
m_map_img.copyTo(m_window_img);
// Draw line center points
for(std::vector<LineDetectionResult>::iterator iter_lcp = map_line_points.begin(); iter_lcp != map_line_points.end(); iter_lcp++)
cv::circle(m_window_img, iter_lcp->centerPos(), 3, CV_RGB(0,0,255), cv::FILLED);
for(std::vector<LineDetectionResult>::iterator iter_lcp = sensor_line_points.begin(); iter_lcp != sensor_line_points.end(); iter_lcp++)
cv::circle(m_window_img, transformPositionWorldToMap(iter_lcp->centerPos()), 1, CV_RGB(0,255,0), cv::FILLED);
// Draw barcode
if(!barcode.label().empty())
{
cv::rectangle(m_window_img, barcode.outerRectMap(), CV_RGB(255,128,0), cv::FILLED);
cv::rectangle(m_window_img, barcode.outerRectMap(), CV_RGB(0,0,0), 1);
cv::putText(m_window_img, barcode.label(), cv::Point(barcode.centerMap().x - 24, barcode.centerMap().y + 16), cv::FONT_HERSHEY_SIMPLEX, 0.5, 2);
}
// Draw robot position and status (green: line or barcode detected, red otherwise)
std::string map_color = ImageUtil::isLinePixel(m_map_img, robot_map_pos) ? "black" : "white";
cv::Scalar robot_color = CV_RGB(255,0,0); // default: display robot position in red color
sick_line_guidance::OLS_Measurement ols_measurement_sensor = m_ols_measurement_sensor.get();
sick_line_guidance::OLS_Measurement ols_measurement_world = m_ols_measurement_world.get();
if(!sensor_line_points.empty() || ols_measurement_sensor.barcode > 0 || ols_measurement_sensor.extended_code > 0)
robot_color = CV_RGB(0,255,0); // line or barcode detected: display robot position in green color
// Draw robot
cv::Point2d robot_pos1 = ImageUtil::getWorldPointInDirection(robot_world_pos, robot_yaw_angle - CV_PI/2, m_sensor_config.line_sensor_detection_width/2);
cv::Point2d robot_pos2 = ImageUtil::getWorldPointInDirection(robot_world_pos, robot_yaw_angle + CV_PI/2, m_sensor_config.line_sensor_detection_width/2);
cv::line(m_window_img, transformPositionWorldToMap(robot_pos1), transformPositionWorldToMap(robot_pos2), robot_color, 1);
cv::circle(m_window_img, robot_map_pos, 3, robot_color, cv::FILLED);
// Print ros time and lcp of main line
std::stringstream info;
info << "[" << std::fixed << std::setprecision(9) << ros::WallTime::now().toSec() << ", " << ros::Time::now().toSec() << "]";
if((ols_measurement_sensor.status&0x2) != 0)
info << ": line[1]: " << std::fixed << std::setprecision(3) << ols_measurement_sensor.position[1] << "," << ols_measurement_sensor.width[1] << " (" << ols_measurement_world.position[1] << "," << ols_measurement_world.width[1] << ")";
cv::putText(m_window_img, info.str(), cv::Point(20, m_map_img.rows-32), cv::FONT_HERSHEY_SIMPLEX, 0.5, 2);
// Display the navigation map
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "NavigationMapper: robot xy-position on map:(" << std::setprecision(1) << std::fixed << robot_map_pos.x << "," << robot_map_pos.y << "," << map_color
<< "), linestate:" << (int)(ols_measurement_sensor.status & 0x7) << ((m_navigation_state == FOLLOW_LINE) ? ", follow_line" : ""));
m_video_write.write(m_window_img);
if(!m_window_name.empty())
{
cv::imshow(m_window_name, m_window_img);
cv::waitKey(1);
}
}
m_message_rate.sleep();
}
}

264
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/navigation_util.cpp Executable file
View File

@@ -0,0 +1,264 @@
/*
* NavigationUtil implements utility functions for 2D/3D-transforms and navigation.
*
* 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 <cmath>
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include <opencv2/core.hpp>
#include "sick_line_guidance_demo/image_util.h"
#include "sick_line_guidance_demo/navigation_util.h"
/*
* returns +1, if a point is on the right side of the robot, and -1 otherwise.
* see https://math.stackexchange.com/questions/274712/calculate-on-which-side-of-a-straight-line-is-a-given-point-located
* for the math under the hood.
* @param[in] xpoint x-position of the point
* @param[in] ypoint y-position of the point
* @param[in] xrobot x-position of the robot
* @param[in] yrobot y-position of the robot
* @param[in] heading heading of the robot
* @return +1 (point is on the right side) or -1 (point is on the left side)
*/
int sick_line_guidance_demo::NavigationUtil::isPointRightFromRobot(double xpoint, double ypoint, double xrobot, double yrobot, double heading)
{
double x1 = xrobot;
double y1 = yrobot;
double x2 = x1 + cos(heading);
double y2 = y1 + sin(heading);
double d = (xpoint - x1) * (y2 - y1) - (ypoint - y1) * (x2 - x1);
return (d > 0) ? +1 : -1;
}
/*
* computes and return the euclidean distance between between a robot position and a line center point,
* i.e. a negative value if the line_center is left from robots position when moving in robot_heading, and
* a positive value if the line_center is right from robots position when moving in robot_heading
* @param[in] robot_pos first point (robots position)
* @param[in] line_center_pos second point (line center position)
* @param[in] robot_heading robots moving direction (i.e. robots yaw angle)
* @param[in] sensor_mounted_right_to_left Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right
* @return euclidean distance between a and b
*/
double sick_line_guidance_demo::NavigationUtil::euclideanDistanceOrientated(const cv::Point2d & robot_pos, const cv::Point2d & line_center_pos, double robot_heading, bool sensor_mounted_right_to_left)
{
double dist = euclideanDistance(robot_pos, line_center_pos);
int sign = isPointRightFromRobot(line_center_pos.x, line_center_pos.y, robot_pos.x, robot_pos.y, robot_heading);
if(!sensor_mounted_right_to_left)
sign = -sign; // sign depends on how the sensor is mounted (orientation relative to heading, cable to the right or to the left), default (demo mounting): true
return sign * dist;
}
/*
* transforms and returns (x,y) position and yaw angle of an odometry message into a readable string.
* @param[in] msg odometry message (input)
* @return (x,y) position and yaw angle of odometry message as a readable string (output)
*/
std::string sick_line_guidance_demo::NavigationUtil::toInfo(const nav_msgs::Odometry::ConstPtr& msg)
{
assert(msg);
std::stringstream info;
double posx = 0, posy = 0, yaw = 0;
toWorldPosition(msg, posx, posy, yaw);
info << "nav_msgs::Odometry={pose=(x=" << std::setprecision(3) << std::fixed << posx << ",y=" << std::setprecision(3) << std::fixed << posy
<< ",yaw=" << std::setprecision(6) << std::fixed << yaw << "=" << std::setprecision(1) << std::fixed << (180.0 * yaw / M_PI) << " deg)}";
return info.str();
}
/*
* transforms and returns (x,y) position and yaw angle of an odometry message into a readable string.
* @param[in] msg odometry message (input)
* @return (x,y) position and yaw angle of odometry message as a readable string (output)
*/
std::string sick_line_guidance_demo::NavigationUtil::toInfo(const nav_msgs::Odometry & msg)
{
std::stringstream info;
double posx = 0, posy = 0, yaw = 0;
toWorldPosition(msg, posx, posy, yaw);
info << "nav_msgs::Odometry={pose=(x=" << std::setprecision(3) << std::fixed << posx << ",y=" << std::setprecision(3) << std::fixed << posy
<< ",yaw=" << std::setprecision(6) << std::fixed << yaw << "=" << std::setprecision(1) << std::fixed << (180.0 * yaw / M_PI) << " deg)}";
return info.str();
}
/*
* transforms and returns a gazebo ModelStates message into a readable string.
* @param[in] msg gazebo ModelStates message (input)
* @param[in] start_idx index to start iteration of names and poses (input, default = 0: print all names and poses)
* @param[in] last_idx index to end iteration of names and poses (input, default = MAX_INT: print all names and poses)
* @return gazebo ModelStates message as a readable string (output)
*/
std::string sick_line_guidance_demo::NavigationUtil::toInfo(const gazebo_msgs::ModelStates::ConstPtr& msg, size_t start_idx, size_t last_idx)
{
assert(msg && msg->pose.size() > 1);
std::stringstream info;
// Print ModelStates names
info << "gazebo_msgs::ModelStates={name=[";
for(size_t n = std::max((size_t)0,start_idx); n < std::min(msg->name.size(), last_idx+1); n++)
{
if(n > start_idx)
info << ",";
info << "\"" << msg->name[n] << "\"";
}
// Print ModelStates pose positions
info << "],pose=[";
for(size_t n = std::max((size_t)0,start_idx); n < std::min(msg->pose.size(), last_idx+1); n++)
{
const geometry_msgs::Point & position = msg->pose[n].position;
const geometry_msgs::Quaternion & quat_msg = msg->pose[n].orientation;
double roll=0, pitch=0, yaw=0;
// Print xy-position
if(n > start_idx)
info << ",";
info << "{x=" << std::setprecision(3) << std::fixed << position.x << ",y=" << std::setprecision(3) << std::fixed << position.y; // position.z ignored
// transform orientation to angles: roll (x-axis rotation), pitch (y-axis rotation), yaw (z-axis rotation, i.e. robots orientation in the xy-plane)
transformOrientationToRollPitchYaw(quat_msg, roll, pitch, yaw);
// Print orientation (yaw angle, i.e. robots rotation in xy-plane)
info << ",yaw=" << std::setprecision(6) << std::fixed << yaw << "=" << std::setprecision(1) << std::fixed << (180.0 * yaw / M_PI) << " deg}"; // roll and pitch ignored
}
info << "]}";
return info.str();
}
/*
* gets the robots xy-position and yaw angle from an odometry message
* @param[in] msg odometry message (input)
* @param[out] world_posx x-position in world coordinates [meter]
* @param[out] world_posy y-position in world coordinates [meter]
* @param[out] yaw_angle orientation (z-axis rotation) in rad
*/
void sick_line_guidance_demo::NavigationUtil::toWorldPosition(const nav_msgs::Odometry & msg, double & world_posx, double & world_posy, double & yaw_angle)
{
world_posx = msg.pose.pose.position.x;
world_posy = msg.pose.pose.position.y;
double roll=0, pitch=0;
transformOrientationToRollPitchYaw(msg.pose.pose.orientation, roll, pitch, yaw_angle);
}
/*
* gets the robots xy-position and yaw angle from an odometry message
* @param[in] msg odometry message (input)
* @param[out] world_posx x-position in world coordinates [meter]
* @param[out] world_posy y-position in world coordinates [meter]
* @param[out] yaw_angle orientation (z-axis rotation) in rad
*/
void sick_line_guidance_demo::NavigationUtil::toWorldPosition(const nav_msgs::Odometry::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle)
{
assert(msg);
world_posx = msg->pose.pose.position.x;
world_posy = msg->pose.pose.position.y;
double roll=0, pitch=0;
transformOrientationToRollPitchYaw(msg->pose.pose.orientation, roll, pitch, yaw_angle);
}
/*
* gets the robots xy-position and yaw angle from a gazebo ModelStates message
* @param[in] msg gazebo ModelStates message (input)
* @param[out] world_posx x-position in world coordinates [meter]
* @param[out] world_posy y-position in world coordinates [meter]
* @param[out] yaw_angle orientation (z-axis rotation) in rad
*/
void sick_line_guidance_demo::NavigationUtil::toWorldPosition(const gazebo_msgs::ModelStates::ConstPtr& msg, double & world_posx, double & world_posy, double & yaw_angle)
{
assert(msg && msg->pose.size() > 1);
world_posx = msg->pose[1].position.x;
world_posy = msg->pose[1].position.y;
double roll=0, pitch=0;
transformOrientationToRollPitchYaw(msg->pose[1].orientation, roll, pitch, yaw_angle);
}
/*
* transforms an orientation from Quaternion to angles: roll (x-axis rotation), pitch (y-axis rotation), yaw (z-axis rotation).
* @param[in] quat_msg gazebo orientation (quaternion, input)
* @param[out] roll angle (x-axis rotation, output)
* @param[out] pitch angle (y-axis rotation, output)
* @param[out] yaw angle (z-axis rotation, output)
*/
void sick_line_guidance_demo::NavigationUtil::transformOrientationToRollPitchYaw(const geometry_msgs::Quaternion & quat_msg, double & roll, double & pitch, double & yaw)
{
tf::Quaternion quat_tf(quat_msg.x, quat_msg.y, quat_msg.z, quat_msg.w);
tf::Matrix3x3 quat_mat(quat_tf);
quat_mat.getRPY(roll, pitch, yaw);
}
/*
* Selects line points within the sensor detection zone from a list of possible line center points
* @param[in] world_line_points list of possible line center points in world coordinates[meter]
* @param[in] robot_world_pos robot position in world coordinates [meter]
* @param[in] line_sensor_detection_width width of sensor detection zone (f.e. 0.180 for an OLS mounted in 0.1m height over ground, 0.130 for the demo system with OLS mounted in 0.065m height)
* @param[in] max_line_cnt max. number of lines (OLS: max. 3 lines)
* @return list of line points within the sensor detection zone
*/
std::vector<sick_line_guidance_demo::LineDetectionResult> sick_line_guidance_demo::NavigationUtil::selectLinePointsWithinDetectionZone(std::vector<sick_line_guidance_demo::LineDetectionResult> & world_line_points,
const cv::Point2d & robot_world_pos, double line_sensor_detection_width, size_t max_line_cnt)
{
std::vector<LineDetectionResult> sensor_line_points;
for(std::vector<LineDetectionResult>::iterator iter_line_points = world_line_points.begin(); iter_line_points != world_line_points.end(); iter_line_points++)
{
if(std::abs(iter_line_points->centerDistance()) < line_sensor_detection_width/2 // line center point within detection zone
&& NavigationUtil::euclideanDistance(robot_world_pos, iter_line_points->startPos()) < line_sensor_detection_width/2 // line start point within detection zone
&& NavigationUtil::euclideanDistance(robot_world_pos, iter_line_points->endPos()) < line_sensor_detection_width/2) // line end point within detection zone
// && std::abs(iter_line_points->lineWidth()) < line_sensor_detection_width) // line width within detection zone
{
sensor_line_points.push_back(*iter_line_points);
}
}
if(sensor_line_points.size() > max_line_cnt)
{
sensor_line_points.resize(max_line_cnt);
}
return sensor_line_points;
}

234
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/ols_measurement_simulator.cpp Executable file
View File

@@ -0,0 +1,234 @@
/*
* ols_measurement_simulator simulates OLS_Measurement messages for sick_line_guidance_demo.
*
* OLS_Measurement_Simulator converts the distance between the simulated robot and
* the optical lines from the navigation map into OLS_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 <string>
#include <vector>
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
#include "sick_line_guidance_demo/image_util.h"
#include "sick_line_guidance_demo/ols_measurement_simulator.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* class OLS_Measurement_Simulator converts the distance between the simulated robot and
* the optical lines from the navigation map into OLS_Measurement messages.
*/
/*
* Constructor
* @param[in] ros_topic_ols_messages ros topic for publishing OLS messages (empty: deactivated)
* @param[in] publish_rate rate to publish OLS measurements (if activated, default: 100)
*/
sick_line_guidance_demo::OLS_Measurement_Simulator::OLS_Measurement_Simulator(ros::NodeHandle* nh, const std::string & ros_topic_ols_messages, double publish_rate)
: m_ols_publish_thread(0), m_ols_publish_rate(publish_rate)
{
m_ros_topic_ols_messages = ros_topic_ols_messages;
m_publish_scheduled = false;
sick_line_guidance::MsgUtil::zero(m_ols_state);
if(nh && !m_ros_topic_ols_messages.empty())
{
m_ols_publish_rate = ros::Rate(publish_rate);
m_ols_publisher = nh->advertise<sick_line_guidance::OLS_Measurement>(m_ros_topic_ols_messages, 1);
m_ols_publish_thread = new boost::thread(&sick_line_guidance_demo::OLS_Measurement_Simulator::runPublishThread, this);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "OLS_Measurement_Simulator: publishing \"" << m_ros_topic_ols_messages << "\"");
}
else
{
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "OLS_Measurement_Simulator: deactivated (ols topic: \"" << m_ros_topic_ols_messages << "\"");
}
}
/*
* Destructor
*/
sick_line_guidance_demo::OLS_Measurement_Simulator::~OLS_Measurement_Simulator()
{
}
/*
* publish the current OLS state
*/
void sick_line_guidance_demo::OLS_Measurement_Simulator::publish(void)
{
if(!m_ros_topic_ols_messages.empty())
{
m_ols_state.header.stamp = ros::Time::now();
m_ols_publisher.publish(m_ols_state);
}
}
/*
* publish the current OLS state in background task (publish with fixed rate)
*/
void sick_line_guidance_demo::OLS_Measurement_Simulator::schedulePublish(void)
{
if(!m_ros_topic_ols_messages.empty())
{
m_publish_scheduled = true;
}
}
/*
* thread callback, just publishes the current OLS state with a const rate
*/
void sick_line_guidance_demo::OLS_Measurement_Simulator::runPublishThread(void)
{
while(ros::ok())
{
if(!m_ros_topic_ols_messages.empty() && m_publish_scheduled)
{
publish();
m_publish_scheduled = false;
}
m_ols_publish_rate.sleep();
}
}
/*
* rounds a double value to a given float precision, f.e. roundPrecision(lcp, 0.001) to round a line center point to millimeter precision.
*/
float sick_line_guidance_demo::OLS_Measurement_Simulator::roundPrecision(double value, double precision)
{
return (float)(precision * std::lround(value / precision));
}
/*
* Initializes ols_state for one line (position, width and status)
*/
void sick_line_guidance_demo::OLS_Measurement_Simulator::setLine(sick_line_guidance::OLS_Measurement & ols_state, float line_position, float line_width)
{
ols_state.position = { 0, line_position, 0 };
ols_state.width = { 0, line_width, 0 };
ols_state.intensity_of_lines = { 0, 100, 0 };
ols_state.quality_of_lines = 100;
ols_state.status = 0x2;
}
/*
* Initializes ols_state for detected lines (position, width and status for 0, 1, 2 or 3 lines)
*/
void sick_line_guidance_demo::OLS_Measurement_Simulator::setLines(sick_line_guidance::OLS_Measurement & ols_state, std::vector<sick_line_guidance_demo::LineDetectionResult> & sensor_line_points)
{
if(sensor_line_points.size() <= 0) // No line detected
{
ols_state.position = { 0, 0, 0 };
ols_state.width = { 0, 0, 0 };
ols_state.intensity_of_lines = { 0, 0, 0 };
ols_state.status = 0;
ols_state.quality_of_lines = 0;
}
else
{
// First (main) line: ols_state.position[1] is always the nearest line, ols_state.position[0] is the line to the left, ols_state.position[2] is the line to the right (in driving direction)
ols_state.position = { 0, roundPrecision(sensor_line_points[0].centerDistance(), 0.001), 0 };
ols_state.width = { 0, roundPrecision(sensor_line_points[0].lineWidth() + 0.001, 0.001), 0 };
ols_state.intensity_of_lines = { 0, 100, 0 };
ols_state.status &= 0xF8;
ols_state.status |= 0x2;
ols_state.quality_of_lines = 100;
if(sensor_line_points.size() > 1) // two or three lines detected -> initialize positions and status depending on the side of the line (left side, right side or both)
{
// Sort from left to right
std::vector<LineDetectionResult> line_points_left_to_right;
line_points_left_to_right.push_back(sensor_line_points[1]); // second line
if(sensor_line_points.size() > 2)
{
line_points_left_to_right.push_back(sensor_line_points[2]); // third line
std::sort(line_points_left_to_right.begin(), line_points_left_to_right.end(), [](const LineDetectionResult & a, const LineDetectionResult & b){ return (a.centerDistance() < b.centerDistance()); });
}
for(std::vector<LineDetectionResult>::iterator iter_line = line_points_left_to_right.begin(); iter_line < line_points_left_to_right.end(); iter_line++)
{
int sensor_line_idx = (iter_line->centerDistance() < 0 ? 0 : 2); // OLS: sensor_line_idx==0: line to the left, sensor_line_idx==2: line to the right
int sensor_line_status = (sensor_line_idx == 0 ? 1 : 4); // OLS: (line status & 1) != 0: line to the left, (line status & 4) != 0: line to the left
if((ols_state.status & sensor_line_status) == 0)
{
ols_state.position[sensor_line_idx] = roundPrecision(iter_line->centerDistance(), 0.001);
ols_state.width[sensor_line_idx] = roundPrecision(iter_line->lineWidth() + 0.001, 0.001);
ols_state.intensity_of_lines[sensor_line_idx] = 100;
ols_state.status |= sensor_line_status;
}
}
}
}
}
/*
* Sets the barcode of an ols_state
*/
void sick_line_guidance_demo::OLS_Measurement_Simulator::setBarcode(sick_line_guidance::OLS_Measurement & ols_state, size_t label, bool flipped)
{
if(label > 255)
{
ols_state.barcode = 255;
ols_state.extended_code = label;
}
else
{
ols_state.barcode = label;
ols_state.extended_code = 0;
}
// status Bit 6: Code flipped, Bit 7: Code valid
if(flipped)
ols_state.status |= (1 << 6);
else
ols_state.status &= ~(1 << 6);
if(ols_state.barcode > 0)
ols_state.status |= (1 << 7);
else
ols_state.status &= ~(1 << 7);
}

62
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/pid.cpp Executable file
View File

@@ -0,0 +1,62 @@
#include "sick_line_guidance_demo/pid.h"
//using namespace std;
/* Object contstructor */
PID_Controller::PID_Controller(double dt, double kp, double ki, double kd){
_dt = dt;
_kp = kp;
_ki = ki;
_kd = kd;
}
/* Function to calculate controller output */
double PID_Controller::calculate_output(double setpoint_value, double actual_value){
_e_z = _e; // store old deviation in e_z (e*z⁻1)
_e = setpoint_value - actual_value; // calculate deviation
_e_int += _e; // integrate
_e_diff = _e - _e_z; //derivate
_yp = _kp * _e; // calculate P-Controller output
_yi = _ki * _dt * _e_int; // calculate I-Controller output
_yd = _kd * _e_diff/_dt; // calculate D-Controller output
return (_yp + _yi + _yd); // calculate PID-Controller output
}
/* Function to set controller parameter */
void PID_Controller::setParams(double dt, double kp, double ki, double kd){
_dt = dt;
_kp = kp;
_ki = ki;
_kd = kd;
}
void PID_Controller::getParams(double& dt, double& kp, double& ki, double& kd){
dt = _dt;
kp = _kp;
ki = _ki;
kd = _kd;
}
void PID_Controller::reset(){
_dt = 0;
_kp = 0;
_ki = 0;
_kd = 0;
_e = 0;
_e_int = 0;
_e_diff = 0;
_e_z = 0;
_yp = 0;
_yi = 0;
_yd = 0;
}
void PID_Controller::clear_state(){
double params[4];
getParams(params[0], params[1], params[2], params[3]);
reset();
setParams(params[0], params[1], params[2], params[3]);
}
PID_Controller::~PID_Controller(){}

221
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/robot_fsm.cpp Executable file
View File

@@ -0,0 +1,221 @@
/*
* RobotFSM implements the state machine to explore and follow a line
* for sick_line_guidance_demo.
* The following RobotStates are executed:
* INITIAL -> EXPLORE_LINE -> FOLLOW_LINE [ -> WAIT_AT_BARCODE -> FOLLOW_LINE ] -> EXIT
* Theses states are implemented in the following classes:
* EXPLORE_LINE: ExploreLineState
* FOLLOW_LINE: FollowLineState
* WAIT_AT_BARCODE: WaitAtBarcodeState
*
* 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 <cmath>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
#include "sick_line_guidance_demo/explore_line_state.h"
#include "sick_line_guidance_demo/follow_line_state.h"
#include "sick_line_guidance_demo/navigation_util.h"
#include "sick_line_guidance_demo/robot_fsm.h"
#include "sick_line_guidance_demo/stop_go_fsm.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* class RobotFSM implements the state machine to explore and follow a line
* for sick_line_guidance_demo.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
/*
* Constructor
* @param[in] nh ros handle
* @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input)
* @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input)
* @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output)
*/
sick_line_guidance_demo::RobotFSM::RobotFSM(ros::NodeHandle* nh, const std::string & ros_topic_ols_messages, const std::string & ros_topic_odometry, const std::string & ros_topic_cmd_vel)
: m_fsm_thread(0), m_fsm_thread_run(false)
{
if(nh && !ros_topic_ols_messages.empty())
{
m_ols_subscriber = nh->subscribe(ros_topic_ols_messages, 1, &sick_line_guidance_demo::RobotFSM::messageCallbackOlsMeasurement, this);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_ols_messages);
}
if(nh && !ros_topic_odometry.empty())
{
m_odom_subscriber = nh->subscribe(ros_topic_odometry, 1, &sick_line_guidance_demo::RobotFSM::messageCallbackOdometry, this);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_odometry);
}
if(nh && !ros_topic_cmd_vel.empty())
{
m_cmd_vel_publisher = nh->advertise<geometry_msgs::Twist>(ros_topic_cmd_vel, 1);
m_fsm_context.setVelocityPublisher(&m_cmd_vel_publisher);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: publishing " << m_cmd_vel_publisher.getTopic());
}
if(nh)
{
m_follow_line = FollowLineState(nh, &m_fsm_context);
m_explore_line = ExploreLineState(nh, &m_fsm_context);
wait_at_barcode_state = WaitAtBarcodeState(nh, &m_fsm_context);
}
}
/*
* Destructor
*/
sick_line_guidance_demo::RobotFSM::~RobotFSM()
{
m_fsm_context.setVelocityPublisher(0);
}
/*
* Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel
*/
void sick_line_guidance_demo::RobotFSM::startFSM(void)
{
// Stop FSM and start a new thread to run the FSM
stopFSM();
m_fsm_thread_run = true;
m_fsm_thread = new boost::thread(&sick_line_guidance_demo::RobotFSM::runFSMthread, this);
}
/*
* Stops the thread to run the final state machine
*/
void sick_line_guidance_demo::RobotFSM::stopFSM(void)
{
if(m_fsm_thread)
{
m_fsm_thread_run = false;
m_fsm_thread->join();
delete(m_fsm_thread);
m_fsm_thread = 0;
}
}
/*
* message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom".
* It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot,
* and transform them invers into world coordinates.
* @param[in] msg odometry message (input)
*/
void sick_line_guidance_demo::RobotFSM::messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
{
m_fsm_context.setOdomState(msg);
}
/*
* message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols".
* It displays the OLS measurement (line info and barcodes), if visualization is enabled.
* @param[in] msg OLS measurement message (input)
*/
void sick_line_guidance_demo::RobotFSM::messageCallbackOlsMeasurement(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg)
{
m_fsm_context.setOlsState(msg);
}
/*
* thread callback, runs the final state machine for sick_line_guidance_demo.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
void sick_line_guidance_demo::RobotFSM::runFSMthread(void)
{
try
{
RobotFSMContext::RobotState robot_state = RobotFSMContext::RobotState::INITIAL;
while(ros::ok() && m_fsm_thread_run && robot_state != RobotFSMContext::RobotState::EXIT && robot_state != RobotFSMContext::RobotState::FATAL_ERROR)
{
switch(robot_state)
{
case RobotFSMContext::RobotState::INITIAL:
robot_state = RobotFSMContext::RobotState::EXPLORE_LINE;
break;
case RobotFSMContext::RobotState::EXPLORE_LINE:
m_explore_line.clear();
robot_state = m_explore_line.run();
break;
case RobotFSMContext::RobotState::FOLLOW_LINE:
m_follow_line.clear();
robot_state = m_follow_line.run();
break;
case RobotFSMContext::RobotState::WAIT_AT_BARCODE:
wait_at_barcode_state.clear();
robot_state = wait_at_barcode_state.run();
break;
case RobotFSMContext::RobotState::FATAL_ERROR:
ROS_ERROR_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): entered state FATAL_ERROR, exiting");
break;
case RobotFSMContext::RobotState::EXIT:
ROS_INFO_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): switched to state EXIT, exiting");
break;
default:
ROS_ERROR_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): entered unsupported state " << (int)robot_state);
break;
}
}
ROS_INFO_STREAM("sick_line_guidance_demo::RobotFSM::runFSMthread(): entered state " << (int)robot_state << ", exiting");
}
catch(const std::exception & exc)
{
std::cerr << "sick_line_guidance_demo::RobotFSM::runFSMthread(): exception \"" << exc.what() << "\"" << std::endl;
}
}

139
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_demo_node.cpp Executable file
View File

@@ -0,0 +1,139 @@
/*
* @brief sick_line_guidance_demo_node simulates the sick_line_guidance demo application.
* It receives robots (x,y) positions, maps them with the navigation map and creates
* OLS measurement messages for iam::robot_fsm.
*
* 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 <string>
#include <vector>
#include "sick_line_guidance_demo/navigation_mapper.h"
#include "sick_line_guidance_demo/robot_fsm.h"
#include "sick_line_guidance_demo/time_format.h"
#include "sick_line_guidance_demo/turtlebot_test_fsm.h"
/*
* @brief sick_line_guidance_demo_node simulates the sick_line_guidance demo application.
* It receives robots (x,y) positions, maps them with the navigation map and creates
* OLS measurement messages for iam::robot_fsm.
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "sick_line_guidance_demo_node");
// Configuration
ros::NodeHandle nh;
sick_line_guidance_demo::LineSensorConfig sensor_config;
int ols_simu = 0; // Simulate ols measurement messages
int visualize = 0; // Enable visualization: visualize==2: visualization+video enabled, map and navigation plots are displayed in a window, visualize==1: video created but not displayed, visualize==0: visualization and video disabled
std::string ros_topic_odometry = "/odom"; // ROS topic for odometry incl. robot positions (input)
std::string ros_topic_ols_messages = "/ols"; // ROS topic for OLS_Measurement messages (simulation: input+output, otherwise input)
std::string ros_topic_cmd_vel = "/cmd_vel"; // ROS topic for cmd_vel messages (output)
std::string navigation_map_file = "demo_map_02.png"; // Navigation map (input)
std::string intrinsics_xml_file = "cam_intrinsic.xml"; // Intrinsics parameter to transform position from navigation map (pixel) to real world (meter) and vice versa (input, cx=cy=660, fx=fy=1)
std::string barcodes_xml_file = "demo_barcodes.xml"; // List of barcodes with label and position (input)
sensor_config.line_sensor_detection_width = 0.130; // Width of the detection area of the line sensor (meter)
sensor_config.line_sensor_scaling_dist = 180.0 / 133.0; // Scaling between physical distance to the line center and measured line center point (measurement: lcp = 180 mm, physical: lcp = 133 mm), depending on mounted sensor height
sensor_config.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)
sensor_config.line_sensor_mounted_right_to_left = true; // Sensor mounted from right to left (true, demo robot configuration) or otherwise from left to right
nh.param("/sick_line_guidance_demo_node/ols_simu", ols_simu, ols_simu);
nh.param("/sick_line_guidance_demo_node/visualize", visualize, visualize);
nh.param("/sick_line_guidance_demo_node/ros_topic_odometry", ros_topic_odometry, ros_topic_odometry);
nh.param("/sick_line_guidance_demo_node/ros_topic_ols_messages", ros_topic_ols_messages, ros_topic_ols_messages);
nh.param("/sick_line_guidance_demo_node/ros_topic_cmd_vel", ros_topic_cmd_vel, ros_topic_cmd_vel);
nh.param("/sick_line_guidance_demo_node/navigation_map_file", navigation_map_file, navigation_map_file);
nh.param("/sick_line_guidance_demo_node/intrinsics_xml_file", intrinsics_xml_file, intrinsics_xml_file);
nh.param("/sick_line_guidance_demo_node/barcodes_xml_file", barcodes_xml_file, barcodes_xml_file);
nh.param("/sick_line_guidance_demo_node/line_sensor_detection_width", sensor_config.line_sensor_detection_width, sensor_config.line_sensor_detection_width);
nh.param("/sick_line_guidance_demo_node/line_sensor_scaling_dist", sensor_config.line_sensor_scaling_dist, sensor_config.line_sensor_scaling_dist);
nh.param("/sick_line_guidance_demo_node/line_sensor_scaling_width", sensor_config.line_sensor_scaling_width, sensor_config.line_sensor_scaling_width);
nh.param("/sick_line_guidance_demo_node/line_sensor_mounted_right_to_left", sensor_config.line_sensor_mounted_right_to_left, sensor_config.line_sensor_mounted_right_to_left);
// Create navigation mapper (transform the robots xy-positions from world/meter into map/pixel position,
// detect lines and barcodes in the map plus their distance to the robot and transform them invers into world coordinates)
sick_line_guidance_demo::NavigationMapper navigation_mapper(&nh, navigation_map_file, intrinsics_xml_file, barcodes_xml_file, ols_simu ? ros_topic_ols_messages : "", sensor_config, visualize);
// Final state machine (FSM) to explore and follow a line. Input: ols and odometry messages, Output: cmd_vel messages
sick_line_guidance_demo::RobotFSM robot_fsm(&nh, ros_topic_ols_messages, ros_topic_odometry, ros_topic_cmd_vel /* + "_synth" */ );
// Subscribe topics and install callbacks
ros::Subscriber odom_message_subscriber = nh.subscribe(ros_topic_odometry, 1, &sick_line_guidance_demo::NavigationMapper::messageCallbackOdometry, &navigation_mapper);
ros::Subscriber ols_message_subscriber = nh.subscribe(ros_topic_ols_messages, 1, &sick_line_guidance_demo::NavigationMapper::messageCallbackOlsMeasurement, &navigation_mapper);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: subscribing " << odom_message_subscriber.getTopic() << " and " << ols_message_subscriber.getTopic());
// Start FSM to explore and follow a line
navigation_mapper.start();
robot_fsm.startFSM();
// Test only: Run a final state machine (FSM) to test Turtlebot with changing velocities, output: geometry_msgs::Twist messages on topic "/cmd_vel"
// sick_line_guidance_demo::TurtlebotTestFSM turtlebot_test_fsm(&nh, "/ols", "/odom", "/cmd_vel", false);
// ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: starting turtlebot_test_fsm_node");
// turtlebot_test_fsm.startFSM(); // test only, do not use for demos
// Run ros event loop
ros::spin();
// Exit
std::cout << "sick_line_guidance_demo: exiting (1/2)" << std::endl;
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: exiting (1/2)");
navigation_mapper.stop();
robot_fsm.stopFSM();
// turtlebot_test_fsm.stopFSM();
std::cout << "sick_line_guidance_demo: exiting (2/2)" << std::endl;
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "sick_line_guidance_demo: exiting (2/2)");
return 0;
}

239
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/sick_line_guidance_watchdog.cpp Executable file
View File

@@ -0,0 +1,239 @@
/*
* @brief sick_line_guidance_watchdog implements a watchdog.
* If no line or barcode is detected for some time (after a watchdog timeout, f.e. 1 second),
* a command or executable is started (f.e. an emergency script to stop a turtlebot
* by killing all nodes).
* Watchdog timeout in seconds and command can be configured in the launchfile.
* Default values are 1 second watchdog timeout and a watchdog command "nohup rosnode kill -a &".
*
* 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 <boost/thread.hpp>
#include <string>
#include <stdlib.h>
#include <nav_msgs/Odometry.h>
#include "sick_line_guidance/OLS_Measurement.h"
#include "sick_line_guidance_demo/navigation_util.h"
namespace sick_line_guidance_demo
{
/*
* class Watchdog implements an emergency kill in case lines and barcodes have been lost for more than 1 second
*/
class Watchdog
{
public:
/*
* Constructor
* @param[in] command watchdog command, executed in case of watchdog timeouts, f.e. "nohup rosnode kill -a"
* @param[in] timeout watchdog timeout in seconds, f.e. 1 second
* @param[in] check_rate rate to check OLS line detected messages, default: 10 per second
* @param[in] barcode_height height of barcode (i.e. area without valid ols line), default: 0.055 (55 mm)
*/
Watchdog(const std::string & command = "", double timeout = 0, double check_rate = 10, double barcode_height = 0.055)
: m_watchdog_command(command), m_watchdog_timeout(timeout), m_check_rate(check_rate), m_barcode_height(barcode_height)
{
if(timeout > FLT_EPSILON)
{
m_check_rate = ros::Rate(check_rate);
m_time_line_detected = ros::Time::now();
m_time_ols_message = ros::Time::now();
m_time_odom_message = ros::Time::now();
m_pos_line_detected = cv::Point2d();
m_pos_odom_message = cv::Point2d();
m_watchdog_thread = new boost::thread(&sick_line_guidance_demo::Watchdog::watchdogThreadCb, this);
}
}
/*
* Message callback for ols messages. Updates the watchdog, if a line is detected ((ols_msg->status & 0x7) != 0)
* or a barcode has been read (
* @param[in] ols_msg ols message with line info
*/
virtual void olsMessageCb(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& ols_msg)
{
if(ols_msg)
m_time_ols_message = ros::Time::now();
if(ols_msg != 0 && (ols_msg->status & 0x7) != 0) // bit 0,1 or 2 of status is set -> line detected
{
m_time_line_detected = m_time_ols_message;
m_pos_line_detected = m_pos_odom_message;
ROS_DEBUG_STREAM("sick_line_guidance_watchdog: ols message received, line detected (ols status: " << (int)ols_msg->status << ")" );
}
else if(ols_msg != 0 && (ols_msg->status & 0x80) != 0) // bit 7 of status is set -> barcode valid
{
m_time_line_detected = m_time_ols_message;
m_pos_line_detected = m_pos_odom_message;
ROS_DEBUG_STREAM("sick_line_guidance_watchdog: ols message received, line detected (ols status: " << (int)ols_msg->status << ")" );
}
else
{
if(ols_msg != 0)
ROS_ERROR_STREAM("sick_line_guidance_watchdog: ols message received, no line detected (ols status: " << (int)ols_msg->status << ")" );
else
ROS_ERROR_STREAM("sick_line_guidance_watchdog: invalid ols message received" );
}
}
/*
* Message callback for odometry messages. Updates the watchdog, whenever an odom message is received
* or a barcode has been read (
* @param[in] odom_msg odometry message
*/
virtual void odomMessageCb(const nav_msgs::Odometry::ConstPtr& odom_msg)
{
if(odom_msg)
{
m_time_odom_message = ros::Time::now();
double posx=0, posy=0, yaw = 0;
sick_line_guidance_demo::NavigationUtil::toWorldPosition(odom_msg, posx, posy, yaw);
m_pos_odom_message = cv::Point2d(posx, posy);
}
}
protected:
/*
* Thread callback, monitors the time of last line or barcode detection and calls the timeout command in case of a watchdog timeout.
*/
void watchdogThreadCb(void)
{
ROS_INFO_STREAM("sick_line_guidance_watchdog: started" );
while(ros::ok())
{
ROS_DEBUG_STREAM("sick_line_guidance_watchdog: running" );
if((ros::Time::now() - m_time_ols_message).toSec() > m_watchdog_timeout)
{
ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, no ols message received." );
break;
}
if((ros::Time::now() - m_time_odom_message).toSec() > m_watchdog_timeout)
{
ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, no odom message received." );
break;
}
if((ros::Time::now() - m_time_line_detected).toSec() > m_watchdog_timeout
&& NavigationUtil::euclideanDistance(m_pos_odom_message, m_pos_line_detected) > m_barcode_height)
{
ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, no line detected." );
break;
}
m_check_rate.sleep();
}
if(ros::ok() && !m_watchdog_command.empty())
{
m_watchdog_command += " &";
ROS_ERROR_STREAM("sick_line_guidance_watchdog: WATCHDOG TIMEOUT, calling system(\"" << m_watchdog_command << "\") ..." );
system(m_watchdog_command.c_str());
}
ROS_INFO_STREAM("sick_line_guidance_watchdog: finished" );
}
std::string m_watchdog_command; // watchdog command, executed in case of watchdog timeouts, default: "nohup rosnode kill -a"
double m_watchdog_timeout; // watchdog timeout in seconds, default: 1 second
ros::Rate m_check_rate; // rate to check OLS line detected messages, default: 10 per second
ros::Time m_time_line_detected; // timestamp of last detected line or barcode
cv::Point2d m_pos_line_detected; // robot position (world coordinates in meter) at last detected line or barcode
ros::Time m_time_ols_message; // timestamp of last ols message
ros::Time m_time_odom_message; // timestamp of last odom message
cv::Point2d m_pos_odom_message; // robot position (world coordinates in meter) at last odom message
double m_barcode_height; // height of barcode (55 mm, area without valid ols line)
boost::thread* m_watchdog_thread; // thread to monitor time of last line/barcode detection, calls the watchdog command in case of watchdog timeouts
};
}
/*
* @brief sick_line_guidance_watchdog subscribes to topic "/ols", monitors the detection of lines and barcodes
* and calls an emergency stop in case of a watchdog timeout.
* Default configuration: system command "nohup rosnode kill -a" (kill all ros nodes) is called
* if no line could be detected for more than 1 second.
* Usage example: "roslaunch sick_line_guidance_demo sick_line_guidance_watchdog.launch"
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "sick_line_guidance_watchdog");
// Configuration
ros::NodeHandle nh;
std::string ols_topic = "/ols"; // ROS topic for OLS_Measurement messages
std::string odom_topic = "/odom"; // ROS topic for odometry messages
std::string watchdog_command = "nohup rosnode kill -a"; // watchdog command, executed in case of watchdog timeouts
double watchdog_timeout = 1.0; // watchdog timeout in seconds
double watchdog_check_frequency = 10; // rate to check OLS messages
double barcode_height = 0.055; // height of barcode (55 mm, area without valid ols line)
nh.param("/sick_line_guidance_watchdog/ols_topic", ols_topic, ols_topic);
nh.param("/sick_line_guidance_watchdog/odom_topic", odom_topic, odom_topic);
nh.param("/sick_line_guidance_watchdog/watchdog_timeout", watchdog_timeout, watchdog_timeout);
nh.param("/sick_line_guidance_watchdog/watchdog_check_frequency", watchdog_check_frequency, watchdog_check_frequency);
nh.param("/sick_line_guidance_watchdog/barcode_height", barcode_height, barcode_height);
nh.param("/sick_line_guidance_watchdog/watchdog_command", watchdog_command, watchdog_command);
// Subscribe topic and install callbacks
sick_line_guidance_demo::Watchdog watchdog(watchdog_command, watchdog_timeout, watchdog_check_frequency, barcode_height);
ros::Subscriber ols_message_subscriber = nh.subscribe(ols_topic, 1, &sick_line_guidance_demo::Watchdog::olsMessageCb, &watchdog);
ros::Subscriber odom_message_subscriber = nh.subscribe(odom_topic, 1, &sick_line_guidance_demo::Watchdog::odomMessageCb, &watchdog);
ROS_INFO_STREAM("sick_line_guidance_watchdog: subscribing " << ols_message_subscriber.getTopic());
ROS_INFO_STREAM("sick_line_guidance_watchdog: subscribing " << odom_message_subscriber.getTopic());
// Run ros event loop
ros::spin();
// Exit
std::cout << "sick_line_guidance_watchdog: exiting..." << std::endl;
ROS_INFO_STREAM("sick_line_guidance_watchdog: exiting...");
return 0;
}

135
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/stop_go_fsm.cpp Executable file
View File

@@ -0,0 +1,135 @@
/*
* StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in stop and go.
*
* 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 <string>
#include <vector>
#include <opencv2/core.hpp>
#include <ros/ros.h>
#include "sick_line_guidance_demo/stop_go_fsm.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* class StopGoVelocityFSM implements a simple state machine, creating cmd_vel messages
* to drive a TurtleBot in stop and go.
*/
/*
* Constructor
*/
sick_line_guidance_demo::StopGoVelocityFSM::StopGoVelocityFSM()
{
// define the state machine
m_state_cnt = 0;
m_next_state_switch = ros::Time(0);
VelocityState state;
state.cmd_vel.linear.x = 0.1;
state.cmd_vel.linear.y = 0;
state.cmd_vel.linear.z = 0;
state.cmd_vel.angular.x = 0;
state.cmd_vel.angular.y = 0;
state.cmd_vel.angular.z = -0.5 * M_PI;
// 1. Increase angular velocity from -PI/2 to +PI/2 per second in 0.03*PI steps each 50 ms
while(state.cmd_vel.angular.z <= 0.5 * M_PI)
{
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(0.05);
state.cmd_vel.angular.z += (0.03 * M_PI);
}
// 2. hold last state for 1 second
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(1.0);
// 3. stop for 3 seconds
state.cmd_vel.linear.x = 0.0;
state.cmd_vel.angular.z = 0.0;
m_vec_vel_states.push_back(state);
m_vec_vel_states.back().duration = ros::Duration(3.0);
m_vec_vel_states.back().cmd_vel.linear.x = 0.00;
m_vec_vel_states.back().cmd_vel.angular.z = 0.00;
}
/*
* Destructor
*/
sick_line_guidance_demo::StopGoVelocityFSM::~StopGoVelocityFSM()
{
}
/*
* Next cycle, internal state is updated, velocity message may switch to next movement
*/
void sick_line_guidance_demo::StopGoVelocityFSM::update(void)
{
ros::Time cur_time = ros::Time::now();
if(!m_next_state_switch.isValid())
{
m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration;
}
if(cur_time >= m_next_state_switch)
{
m_state_cnt = ((m_state_cnt + 1) % (m_vec_vel_states.size()));
m_next_state_switch = cur_time + m_vec_vel_states[m_state_cnt].duration;
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "StopGoVelocityFSM: state switched, cmd_vel: linear.x="
<< std::fixed << std::setprecision(3) << (m_vec_vel_states[m_state_cnt].cmd_vel.linear.x) << ", angular.z=" << (m_vec_vel_states[m_state_cnt].cmd_vel.angular.z/M_PI) << "*PI");
}
}
/*
* Returns the cmd_vel message for the current movement
*/
geometry_msgs::Twist sick_line_guidance_demo::StopGoVelocityFSM::getVelocity(void)
{
return m_vec_vel_states[m_state_cnt].cmd_vel;
}

217
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm.cpp Executable file
View File

@@ -0,0 +1,217 @@
/*
* TurtlebotTestFSM implements a state machine to generate cmd_vel messages
* with varying velocities to test the motor control of a TurtleBot.
*
* 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 <cmath>
#include <iomanip>
#include <iostream>
#include <string>
#include <vector>
#include <math.h>
#include <ros/ros.h>
#include <tf/transform_datatypes.h>
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
#include "sick_line_guidance_demo/figure_eight_fsm.h"
#include "sick_line_guidance_demo/navigation_util.h"
#include "sick_line_guidance_demo/turtlebot_test_fsm.h"
#include "sick_line_guidance_demo/stop_go_fsm.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* class TurtlebotTestFSM implements a state machine to generate cmd_vel messages
* with varying velocities to test the motor control of a TurtleBot.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
/*
* Constructor
* @param[in] nh ros handle
* @param[in] ros_topic_ols_messages ROS topic for OLS_Measurement messages (input)
* @param[in] ros_topic_odometry ROS topic for odometry incl. robot positions (input)
* @param[in] ros_topic_cmd_vel ROS topic for cmd_vel messages (output)
* @param[in] print_errors true (default): print high latency by error message (print by info message otherwise)
*/
sick_line_guidance_demo::TurtlebotTestFSM::TurtlebotTestFSM(ros::NodeHandle* nh, const std::string & ros_topic_ols_messages, const std::string & ros_topic_odometry, const std::string & ros_topic_cmd_vel, bool print_errors)
: m_print_errors(print_errors), m_cmd_vel_publish_rate(20), m_fsm_thread(0), m_fsm_thread_run(false), m_cur_odom_timestamp(0), m_cur_velocity_timestamp(0), m_timestamp_stopped(0)
{
m_cmd_vel_publish_rate = ros::Rate(20.0); // cmd_vel messages are published with 20 Hz by default
if(nh && !ros_topic_ols_messages.empty())
{
m_ols_subscriber = nh->subscribe(ros_topic_ols_messages, 1, &sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOlsMeasurement, this);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_ols_messages);
}
if(nh && !ros_topic_odometry.empty())
{
m_odom_subscriber = nh->subscribe(ros_topic_odometry, 1, &sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOdometry, this);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: subscribing " << ros_topic_odometry);
}
if(nh && !ros_topic_cmd_vel.empty())
{
m_cmd_vel_publisher = nh->advertise<geometry_msgs::Twist>(ros_topic_cmd_vel, 1);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: publishing " << m_cmd_vel_publisher.getTopic());
}
}
/*
* Destructor
*/
sick_line_guidance_demo::TurtlebotTestFSM::~TurtlebotTestFSM()
{
}
/*
* Start thread to run the final state machine. Read messages form ols and odom topics, publish messages to cmd_vel
*/
void sick_line_guidance_demo::TurtlebotTestFSM::startFSM(void)
{
// Stop FSM and start a new thread to run the FSM
stopFSM();
m_fsm_thread_run = true;
m_fsm_thread = new boost::thread(&sick_line_guidance_demo::TurtlebotTestFSM::runFSMthread, this);
}
/*
* Stops the thread to run the final state machine
*/
void sick_line_guidance_demo::TurtlebotTestFSM::stopFSM(void)
{
if(m_fsm_thread)
{
m_fsm_thread_run = false;
m_fsm_thread->join();
delete(m_fsm_thread);
m_fsm_thread = 0;
}
}
/*
* message callback for odometry messages. This function is called automatically by the ros::Subscriber after subscription of topic "/odom".
* It transforms the robots xy-positions from world/meter into map/pixel position, detect lines and barcodes in the map plus their distance to the robot,
* and transform them invers into world coordinates.
* @param[in] msg odometry message (input)
*/
void sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
{
if(m_cur_odom_timestamp.isValid() && m_cur_velocity_timestamp.isValid())
{
ros::Time time_now = ros::Time::now();
double posx1 = 0, posx2 = 0, posy1= 0, posy2= 0, yaw1 = 0, yaw2 = 0;
NavigationUtil::toWorldPosition(m_cur_odom, posx1, posy1, yaw1);
NavigationUtil::toWorldPosition(msg, posx2, posy2, yaw2);
double dt = (time_now - m_cur_odom_timestamp).toSec();
double odom_vel_linear = NavigationUtil::euclideanDistance(cv::Point2d(posx1, posy1), cv::Point2d(posx2, posy2)) / dt;
double odom_vel_yaw = NavigationUtil::deltaAngle(yaw1, yaw2) / dt;
if(std::abs(m_cur_velocity.linear.x) < 0.001 && std::abs(m_cur_velocity.angular.z) < 0.001)
{
ROS_INFO_STREAM("RobotFSM: cmd_vel.linear.x=" << m_cur_velocity.linear.x << ", cmd_vel.angular.z=" << m_cur_velocity.angular.z << ", cmd_vel.time=" << sick_line_guidance_demo::TimeFormat::formatDateTime(m_cur_velocity_timestamp));
}
if(std::abs(m_cur_velocity.linear.x) < 0.001 && std::abs(m_cur_velocity.angular.z) < 0.001 && (std::abs(odom_vel_linear) >= 0.002 || std::abs(odom_vel_yaw) >= 0.002))
{
double seconds_stopped = (time_now - m_timestamp_stopped).toSec();
std::stringstream info;
info << "RobotFSM: cmd_vel.linear.x=" << m_cur_velocity.linear.x << ", cmd_vel.angular.z=" << m_cur_velocity.angular.z << ", cmd_vel.time=" << sick_line_guidance_demo::TimeFormat::formatDateTime(m_cur_velocity_timestamp)
<< ", odom_vel_linear=" << odom_vel_linear << ", odom_vel_yaw=" << odom_vel_yaw
<< ", stopped " << seconds_stopped << " seconds ago (now: " << sick_line_guidance_demo::TimeFormat::formatDateTime(time_now)
<< ", stoppped: " << sick_line_guidance_demo::TimeFormat::formatDateTime(m_timestamp_stopped) << ")";
if(m_print_errors && std::abs(odom_vel_linear) >= 0.002 && seconds_stopped >= 1.0)
ROS_ERROR_STREAM(info.str());
else if(m_print_errors)
ROS_WARN_STREAM(info.str());
else
ROS_INFO_STREAM(info.str());
}
}
m_cur_odom = *msg;
m_cur_odom_timestamp = ros::Time::now();
}
/*
* message callback for OLS measurement messages. This function is called automatically by the ros::Subscriber after subscription of topic "/ols".
* It displays the OLS measurement (line info and barcodes), if visualization is enabled.
* @param[in] msg OLS measurement message (input)
*/
void sick_line_guidance_demo::TurtlebotTestFSM::messageCallbackOlsMeasurement(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg)
{
}
/*
* thread callback, runs the final state machine for sick_line_guidance_demo.
* Input: ols and odometry messages
* Output: cmd_vel messages
*/
void sick_line_guidance_demo::TurtlebotTestFSM::runFSMthread(void)
{
// sick_line_guidance_demo::FigureEightVelocityFSM drive_figure_eight_velocity;
sick_line_guidance_demo::StopGoVelocityFSM drive_velocity_fsm;
geometry_msgs::Twist last_velocity;
last_velocity.linear.x = 0;
last_velocity.angular.z = 0;
while(ros::ok() && m_fsm_thread_run)
{
drive_velocity_fsm.update();
m_cur_velocity = drive_velocity_fsm.getVelocity();
m_cur_velocity_timestamp = ros::Time::now();
if((std::abs(last_velocity.linear.x) >= 0.001 || std::abs(last_velocity.angular.z) >= 0.001)
&& std::abs(m_cur_velocity.linear.x) < 0.001 && std::abs(m_cur_velocity.angular.z) < 0.001)
{
m_timestamp_stopped = m_cur_velocity_timestamp;
}
m_cmd_vel_publisher.publish(m_cur_velocity);
last_velocity = m_cur_velocity;
m_cmd_vel_publish_rate.sleep();
}
}

94
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/turtlebot_test_fsm_node.cpp Executable file
View File

@@ -0,0 +1,94 @@
/*
* @brief turtlebot_test_fsm_node implements a simple state machine,
* generating cmd_vel messages to drive the TurtleBot with varying velocities,
* f.e. a figure of eight.
* A simple test for TurtleBot functionality and stability.
*
* 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 <string>
#include <vector>
#include "sick_line_guidance_demo/turtlebot_test_fsm.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* @brief turtlebot_test_fsm_node implements a simple state machine,
* generating cmd_vel messages to drive the TurtleBot with
* varying velocities, f.e. a figure of eight.
* Simple test for TurtleBot functionality and stability.
*/
int main(int argc, char** argv)
{
// ros init
ros::init(argc, argv, "turtlebot_test_fsm_node");
ros::NodeHandle nh;
// Run a final state machine (FSM) to drive a figure of eight.
// Output: geometry_msgs::Twist messages on topic "/cmd_vel"
sick_line_guidance_demo::TurtlebotTestFSM turtlebot_test_fsm(&nh, "/ols", "/odom", "/cmd_vel");
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "turtlebot_test_fsm_node: starting");
turtlebot_test_fsm.startFSM();
// Run ros event loop
ros::spin();
// Exit
std::cout << "turtlebot_test_fsm_node: stopping..." << std::endl;
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "turtlebot_test_fsm_node: stopping...");
turtlebot_test_fsm.stopFSM();
std::cout << "turtlebot_test_fsm_node: exiting..." << std::endl;
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "turtlebot_test_fsm_node: exiting...");
return 0;
}

188
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/velocity_ctr.cpp Executable file
View File

@@ -0,0 +1,188 @@
/*
* velocity_ctr controls angular velocity to avoid acceleration and changing velocity.
* velocity_ctr increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached.
*
* 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 <cfloat>
#include <opencv2/core.hpp>
#include <string>
#include <vector>
#include <ros/ros.h>
#include "sick_line_guidance_demo/adjust_heading.h"
#include "sick_line_guidance_demo/velocity_ctr.h"
/*
* class AngularZCtr controls angular velocity to avoid acceleration and changing velocity.
* It increases/decreases velocity.angular.z smooth and slowly until a desired yaw angle is reached.
*/
/*
* Constructor
*/
sick_line_guidance_demo::AngularZCtr::AngularZCtr()
{
m_dest_yaw_angle = 0; // robots desired heading
m_dest_vel_angular_z = 0; // desired velocity.angular.z (max. value to increase/decrease m_cur_vel_angular_z)
m_cur_yaw_angle = 0; // robots current heading
m_cur_vel_angular_z = 0; // current velocity.angular.z (increased resp. decreased from 0.0 to m_dest_vel_angular_z)
m_start_yaw_angle = 0; // robots heading at start time
m_start_time = ros::Time(0); // timestamp at start
m_update_cnt = -1; // just counts the number of updates (cycles) required to reach the destination heading, or -1 if not started
m_number_start_cyles_with_zero_angular_z = 0; // number of start cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z
m_delta_angular_z = 0.05 * M_PI / 4; // increase/decrease velocity.angular.z by m_delta_angular_z until delta_angle_now is smaller than some epsilon (default: 0.05 * M_PI / 4)
}
/*
* Destructor
*/
sick_line_guidance_demo::AngularZCtr::~AngularZCtr()
{
}
/*
* Starts a new control cycle with smooth transition from cur_yaw_angle and velocity.angular.z = 0 to
* dest_yaw_angle and dest_vel_angular_z.
* @param[in] cur_yaw_angle: current yaw angle (robots current heading)
* @param[in] dest_yaw_angle: destination yaw angle (robots desired heading)
* @param[in] dest_vel_angular_z: destination velocity.angular.z
* @param[in] line_detected: if true (default), start with 5 cycles velocity.angular.z=0 before increasing velocity.angular.z to dest_vel_angular_z, otherwise there's only one cycle with velocity.angular.z=0
* @return first value of velocity.angular.z after start, default: 0
*/
double sick_line_guidance_demo::AngularZCtr::start(double cur_yaw_angle, double dest_yaw_angle, double dest_vel_angular_z, bool line_detected)
{
if(isRunning())
stop();
m_dest_yaw_angle = dest_yaw_angle;
m_dest_vel_angular_z = dest_vel_angular_z;
m_cur_yaw_angle = cur_yaw_angle;
m_cur_vel_angular_z = 0;
m_start_yaw_angle = cur_yaw_angle;
m_number_start_cyles_with_zero_angular_z = line_detected ? 5 : 1;
m_start_time = ros::Time::now();
m_update_cnt = 0;
return m_cur_vel_angular_z;
}
/*
* Stops angular.z control, if currently running.
*/
void sick_line_guidance_demo::AngularZCtr::stop()
{
m_update_cnt = -1;
}
/*
* Update velocity.angular.z control with the current yaw angle.
* @param[in] cur_yaw_angle: current yaw angle (robots current heading)
*/
void sick_line_guidance_demo::AngularZCtr::update(double cur_yaw_angle)
{
if(m_update_cnt >= 0) // otherwise not running
{
// update current heading
m_cur_yaw_angle = cur_yaw_angle;
m_update_cnt++;
// compute new velocity.angular.z
if(m_update_cnt <= m_number_start_cyles_with_zero_angular_z) // currently stopping before increasing/decreasing velocity
{
m_cur_vel_angular_z = 0;
return;
}
if(std::abs(m_dest_vel_angular_z) < FLT_EPSILON)
{
m_update_cnt = -1; // finish after initial wait, nothing to be done
return;
}
// Check: destination heading reached?
double delta_angle_start = AdjustHeading::deltaAngle(m_start_yaw_angle, m_dest_yaw_angle);
double delta_angle_now = AdjustHeading::deltaAngle(cur_yaw_angle, m_dest_yaw_angle);
if((delta_angle_start > 0 && delta_angle_now < 0) || (delta_angle_start < 0 && delta_angle_now > 0) || std::abs(delta_angle_now) < 0.1 * M_PI / 180)
{
m_update_cnt = -1; // m_dest_yaw_angle reached, finished
return;
}
// increase/decrease velocity until delta_angle_now is smaller than some epsilon
m_cur_vel_angular_z += m_delta_angular_z * ((delta_angle_now >= 0) ? 1 : -1);
if(m_cur_vel_angular_z < -std::abs(m_dest_vel_angular_z))
m_cur_vel_angular_z = -std::abs(m_dest_vel_angular_z);
if(m_cur_vel_angular_z > std::abs(m_dest_vel_angular_z))
m_cur_vel_angular_z = std::abs(m_dest_vel_angular_z);
}
}
/*
* Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until
* dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z)
* is reached.
*/
bool sick_line_guidance_demo::AngularZCtr::isRunning(void)
{
return (m_update_cnt >= 0);
}
/*
* Returns true, if AngularZCtr is currently running, i.e. velocity.angular.z is slowly increased/decreased until
* dest_yaw_angle (destination yaw angle, robots heading) and dest_vel_angular_z (destination velocity.angular.z)
* is reached. In this case, output parameter angular_z is set to the new velocity.angular.z to be published,
* and true is returned. Otherwise, false is returned and angular_z remains unchanged.
* @param[out] angular_z: velocity.angular.z to be published (if true returned), otherwise left untouched.
*/
bool sick_line_guidance_demo::AngularZCtr::isRunning(double & angular_z)
{
if(m_update_cnt >= 0) // otherwise not running
{
angular_z = m_cur_vel_angular_z;
return true;
}
return false;
}

126
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/src/wait_at_barcode_state.cpp Executable file
View File

@@ -0,0 +1,126 @@
/*
* WaitAtBarcodeState implements the state to wait at a barcode for a configurable amount of time.
*
* 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_demo/wait_at_barcode_state.h"
#include "sick_line_guidance_demo/navigation_util.h"
#include "sick_line_guidance_demo/time_format.h"
/*
* Constructor
* @param[in] nh ros handle
* @param[in] context shared fsm context
*/
sick_line_guidance_demo::WaitAtBarcodeState::WaitAtBarcodeState(ros::NodeHandle* nh, RobotFSMContext* context) : m_fsm_context(context), m_stop_wait_time(10)
{
if(nh)
{
// Configuration of wait at barcode state
ros::param::getCached("/wait_at_barcode_state/stopWaitSeconds", m_stop_wait_time); // time in seconds to stop at a barcode, default: 10 seconds
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "Configuration wait at barcode state: stopWaitSeconds=" << m_stop_wait_time);
}
}
/*
* Destructor
*/
sick_line_guidance_demo::WaitAtBarcodeState::~WaitAtBarcodeState()
{
m_fsm_context = 0;
}
/*
* Clears all internal states
*/
void sick_line_guidance_demo::WaitAtBarcodeState::clear(void)
{
}
/*
* Runs the wait at barcode state for a configurable amount of time.
* Toggles follow_left_turnout flag at barcode label 101.
* @return FOLLOW_LINE, or EXIT in case ros::ok()==false.
*/
sick_line_guidance_demo::RobotFSMContext::RobotState sick_line_guidance_demo::WaitAtBarcodeState::run(void)
{
assert(m_fsm_context);
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: entering WaitAtBarcodeState");
ros::Time stop_wait_time = ros::Time::now() + ros::Duration(m_stop_wait_time);
geometry_msgs::Twist velocityMessage;
velocityMessage.linear.x = 0;
velocityMessage.linear.y = 0;
velocityMessage.linear.z = 0;
velocityMessage.angular.x = 0;
velocityMessage.angular.y = 0;
velocityMessage.angular.z = 0;
// toggle follow_left_turnout at barcode 101: if true, follow a turnout on the left side, otherwise keep the main line
if(m_fsm_context->getCurBarcode() == 101)
{
bool follow_left_turnout = m_fsm_context->getFollowLeftTurnout();
m_fsm_context->setFollowLeftTurnout(follow_left_turnout ? false : true);
}
ros::Rate wait_rate = ros::Rate(20);
while(ros::ok())
{
if(ros::Time::now() > stop_wait_time)
return RobotFSMContext::RobotState::FOLLOW_LINE;
wait_rate.sleep();
m_fsm_context->publish(velocityMessage);
}
ROS_INFO_STREAM(sick_line_guidance_demo::TimeFormat::formatDateTime() << "RobotFSM: leaving WaitAtBarcodeState");
return RobotFSMContext::RobotState::EXIT; // ros::ok() == false
}

42
Devices/Packages/sick_line_guidance/turtlebotDemo/sick_line_guidance_demo/yaml/sick_line_guidance_demo.yaml Executable file
View File

@@ -0,0 +1,42 @@
#
# Configuration file for sick_line_guidance_demo
#
# Configuration of PID controller
pid_controller:
kp: 30
ki: 0
kd: 0
setpoint: 0.016
# Configuration of follow line state
follow_line_state:
followLineRate: 20 # frequency to update follow line state, default 20 Hz
followSpeed: 0.04 # default linear velocity to follow a line
noLineTime: 5 # time in seconds before switching to state explore line because of lost line
sensorLineWidth: 0.029 # measured line width at 0 degree, 29 mm for an OLS mounted 65 mm over ground, 20 mm for an OLS mounted 100 mm over ground
sensorLineMeasurementJitter: 0.003 # tolerate some line measurement jitter when adjusting the heading
adjustHeadingAngularZ: 0.07854 # 0.07854 = 0.1 * M_PI / 4 # velocity.angular.z to adjust robots heading
adjustHeadingLcpDeviationThresh: 0.04 # start to adjust heading, if the line distance increases over time (deviation of 1D-regression of line center points is above threshold lcpDeviationThresh)
adjustHeadingLcpThresh: 0.04 # start to adjust heading, if the line distance in meter (abs value) is above this threshold
adjustHeadingDeltaAngleEpsilon: 0.001745 # 0.001745 = 0.1 * M_PI / 180; # search stops, if the difference between current and desired yaw angle is smaller than delta_angle_epsilon
adjustHeadingMinDistanceToLastAdjust: 0.01 # move at least some cm before doing next heading adjustment
olsMessageTimeout: 0.5 # timeout for ols messages, robot stops and waits, if last ols message was received more than <timeout> seconds ago
odomMessageTimeout: 0.5 # timeout for odom messages, robot stops and waits, if last ols message was received more than <timeout> seconds ago
# Configuration of explore line state
explore_line_state:
exploreLineRate: 20 # frequency to update explore line state, default 20 Hz
exploreSpeed: 0.04 # default linear velocity to explore a line
olsMessageTimeout: 0.5 # timeout for ols messages, robot stops and waits, if last ols message was received more than <timeout> seconds ago
odomMessageTimeout: 0.5 # timeout for odom messages, robot stops and waits, if last ols message was received more than <timeout> seconds ago
# Configuration of wait at barcode state
wait_at_barcode_state:
stopWaitSeconds: 10 # time in seconds to stop at a barcode
# Configuration of error simulation (test only)
error_simulation:
burst_no_line_duration: 0.0 # 1.0 # error simulation (test only), duration of "no line detected" bursts in seconds, default 0.0 (disabled)
burst_no_line_frequency: 0.0 # 0.1 # error simulation (test only), frequency of "no line detected" bursts in 1/seconds, default 0.0 (disabled)

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

@@ -0,0 +1,17 @@
#!/bin/bash
pushd ../../../../../
source /opt/ros/melodic/setup.bash
./src/sick_line_guidance_demo/test/scripts/killall.bash
killall rosmaster ; sleep 1
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
if [ -f ./sick_line_guidance_demo.avi ] ; then rm -f ./sick_line_guidance_demo.avi ; fi

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

@@ -0,0 +1,23 @@
#!/bin/bash
# init ros environment
source /opt/ros/melodic/setup.bash
source ../../../../../devel/setup.bash
source ../../../../../install/setup.bash
# start edit resource-files
gedit ./rungeneric.bash ../../sick_line_guidance_demo/launch/sick_line_guidance_demo_node.launch ../../sick_line_guidance_demo/yaml/sick_line_guidance_demo.yaml &
# start clion
echo -e "Starting clion...\nNote in case of clion/cmake errors:"
echo -e " Click 'File' -> 'Reload Cmake Project'"
echo -e " cmake/clion: Project 'XXX' tried to find library '-lpthread' -> delete 'thread' from find_package(Boost ... COMPONENTS ...) in CMakeLists.txt"
echo -e " rm -rf ../../../.idea # removes all clion settings"
echo -e " rm -f ~/CMakeCache.txt"
echo -e " 'File' -> 'Settings' -> 'CMake' -> 'CMake options' : CATKIN_DEVEL_PREFIX=~/TASK007_PA0100_ROS_Treiber_Spurfuehrungsssensor/catkin_ws/devel"
echo -e " 'File' -> 'Settings' -> 'CMake' -> 'Generation path' : ../clion_build"
pushd ../../../../..
~/Public/clion-2018.3.3/bin/clion.sh ./src &
popd

46
Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/gitCloneInstall.bash Executable file
View File

@@ -0,0 +1,46 @@
#!/bin/bash
#
# Get all packages for sick_line_guidance_demo,
# build and install.
#
# Clean up
# rosclean purge -y
# rm -rf ./build ./devel ./install
# rm -rf ~/.ros/*
# catkin clean --yes --all-profiles --verbose
# catkin_make clean
#
# Install ros packages for turtlebot
pushd ../../../../../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
# Install 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
# Install 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
# Install sick_line_guidance package
git clone https://github.com/ros-planning/random_numbers.git
git clone https://github.com/SICKAG/sick_line_guidance.git
# Install 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
# git clone https://github.com/ethz-asl/schweizer_messer.git # toolbox including timing utilities
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
# Build and install
cd ..
catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON"
source ./install/setup.bash
popd

5
Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash Executable file
View File

@@ -0,0 +1,5 @@
#!/bin/bash
echo -e "#\n# runsimu.bash: Stopping all rosnodes...\n# rosnode kill -a ; sleep 1 ; killall robot_fsm rqt_plot gzclient gzserver ; sleep 3\n#"
rosnode kill -a ; sleep 1 ; killall robot_fsm ; killall rqt_plot gzclient ; sleep 1 ; killall gzserver ; sleep 3 # ; killall rosmaster ; sleep 1

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

@@ -0,0 +1,41 @@
#!/bin/bash
#
# Build and install sick_line_guidance_demo.
# Use ./gitCloneInstall.bash to install required packages.
#
# delete old logfiles
pushd ../../../../..
rm -rf install/lib/sick_line_guidance/*
rm -rf install/lib/sick_line_guidance_demo/*
rm -rf install/share/sick_line_guidance/*
rm -rf install/share/sick_line_guidance_demo/*
rm -f build/catkin_make_install.log
# make install, use cmake option TURTLEBOT_DEMO="ON" to include the sick_line_guidance_demo packages in folder turtlebotDemo
source /opt/ros/melodic/setup.bash
catkin_make --cmake-args -DTURTLEBOT_DEMO="ON" 2>&1 | tee -a build/catkin_make_install.log
catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON" 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_demo
# catkin_lint -W1 src/sick_line_guidance
# catkin_lint -W1 src/sick_line_guidance_demo/sick_line_guidance_demo
# 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

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

@@ -0,0 +1,10 @@
#!/bin/bash
echo -e "makeall.bash: build and install ros sick_line_guidance driver and demo"
sudo echo -e "makeall.bash started."
# sudo apt-get install python-catkin-lint
source /opt/ros/melodic/setup.bash
./cleanup.bash
./makepcan.bash
./make.bash
echo -e "makeall.bash finished."

41
Devices/Packages/sick_line_guidance/turtlebotDemo/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.7.0 ] ; then
pushd ../../../../../../peak_systems/peak-linux-driver-8.7.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

18
Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/plotsimu.bash Executable file
View File

@@ -0,0 +1,18 @@
#!/bin/bash
source ./simu_env.bash
source ../../../../../install/setup.bash
# rosrun rviz rviz &
# roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch &
# rqt_plot /gazebo/model_states/pose[1]/position/x /gazebo/model_states/pose[1]/position/y & # display turtlebots (x,y) position over time
rostopic list
rostopic echo /cmd_vel &
rostopic echo /fsm &
rostopic echo /iam_state &
# rostopic echo /odom &
# rostopic echo /ols &
# rostopic echo /gazebo/model_states
# rostopic echo /gazebo/link_states

142
Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/rungeneric.bash Executable file
View File

@@ -0,0 +1,142 @@
#!/bin/bash
#
# set environment, clear screen and logfiles
#
if [ ! -d ./log ] ; then mkdir -p ./log ; fi
if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi
printf "\033c"
rm -rf ./log/*
source ./simu_env.bash
pushd ../../../../..
rm -rf ~/.ros/log/*
# printf "\033c"
source /opt/ros/melodic/setup.bash
source ./install/setup.bash
./src/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash
export LIBPROFILER=/usr/lib/x86_64-linux-gnu/libprofiler.so.0 # path to libprofiler.so.0 may depend on os version
#
# Start fsm, driver, nodes, tools and run the sick_line_guidance_demo simulation
#
# roscore &
# sleep 10 # wait for roscore startup processes
if [ "$TURTLEBOT_SIMULATION" == "1" ] ; then
# Start gazebo / turtlebot simulator
sleep 1 ; echo -e "#\n# runsimu.bash: starting gazebo ..."
sleep 1 ; roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch &
sleep 10 # wait until gazebo has started ...
# Virtual navigation:
# roslaunch turtlebot3_gazebo turtlebot3_world.launch
# roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS='ols_simu:=1'
else # "$TURTLEBOT_SIMULATION" == "0"
if [ "$TURTLEBOT_STOP_AT_START" == "1" ] ; then # Stop turtlebot (set all velocities to 0 and restart turtlebot)
roslaunch turtlebot3_bringup turtlebot3_robot.launch &
sleep 15 ; rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' # stop the turtlebot
sleep 1 ; rosnode kill turtlebot3_core ; sleep 1 ; rosnode kill turtlebot3_diagnostics
fi
# Init turtlebot
sleep 5 ; roslaunch turtlebot3_bringup turtlebot3_robot.launch &
sleep 15
# Start OLS20 driver or simulated OLS messages
if [ "$TURTLEBOT_OLS_SIMULATION" == "1" ] ; then # Run TurtleBot with simulated OLS messages
export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS='ols_simu:=1'
else # "$TURTLEBOT_OLS_SIMULATION" == "0" # CAN initialization and start OLS20 driver
# Init CAN and start OLS20 driver
sudo ip link set can0 up type can bitrate 125000 # configure can0
ip -details link show can0 # check status can0
# Start OLS20 driver
sleep 5 ; roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml > ~/.ros/log/sick_line_guidance_ols20.log &
sleep 10 ; export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS='ols_simu:=0'
fi # "$TURTLEBOT_OLS_SIMULATION"
fi # "$TURTLEBOT_SIMULATION"
# Start sick_line_guidance_demo simulation
export SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS=$SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS' visualize:='$TURTLEBOT_VISUALIZE
echo -e "#\n# runsimu.bash: starting simulation:\n# roslaunch sick_line_guidance_demo sick_line_guidance_demo_node.launch $SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS \n#"
if [ "$TURTLEBOT_PROFILING" == "1" ] ; then # debug and profile sick_line_guidance_demo
rm -f /tmp/sick_line_guidance_demo.prof* # remove previous profile logs
env CPUPROFILE=/tmp/sick_line_guidance_demo.prof LD_PRELOAD=$LIBPROFILER roslaunch sick_line_guidance_demo sick_line_guidance_demo_node.launch $SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS &
else # run sick_line_guidance_demo
roslaunch -v --screen sick_line_guidance sick_line_guidance_demo_node.launch $SICK_LINE_GUIDANCE_DEMO_LAUNCH_ARGS 2>&1 | tee ~/.ros/log/sick_line_guidance_demo_node.log &
fi
sleep 10
# Start sick_line_guidance watchdog run emergency exit to stop the TurtleBot after line lost for more than 1 second
if ! [ "$TURTLEBOT_START_WATCHDOG_AFTER" == "" ] && [ $TURTLEBOT_START_WATCHDOG_AFTER -ge 0 ] ; then
sleep $TURTLEBOT_START_WATCHDOG_AFTER # start watchdog after a simulated OLS is close enough to a line
roslaunch -v --screen sick_line_guidance sick_line_guidance_watchdog.launch 2>&1 | tee ~/.ros/log/watchdog.log & # run watchdog
fi
#
# Run sick_line_guidance_demo for a while: just wait for key 'q' or 'Q'
# while ros nodes sick_line_guidance_demo_node and Robot_FSM are running
#
while true ; do
echo -e "FSM and sick_line_guidance_demo simulation running. Press 'q' to exit..." ; read -t 0.1 -n1 -s key
if [[ $key = "q" ]] || [[ $key = "Q" ]]; then break ; fi
done
#
# Stop turtlebot, fsm and sick_line_guidance_demo
#
rosnode kill sick_line_guidance_demo_node
sleep 0.5 ; rostopic pub --once /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0,z: 0.0}}' ; sleep 0.5 # stop the turtlebot
./src/sick_line_guidance/turtlebotDemo/test/scripts/killall.bash
#
# Check errors and warnings in ros logfiles
#
echo -e "\n#\n# sick_line_guidance_demo simulation finished.\n#\n"
grep "\[ WARN\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*.log >> ~/.ros/log/ros_log_warnings.txt
grep "\[ERROR\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*.log >> ~/.ros/log/ros_log_errors.txt
echo -e "\nSimulation warnings and errors:\n"
cat ~/.ros/log/ros_log_warnings.txt
cat ~/.ros/log/ros_log_errors.txt
#
# Profiling
#
if [ "$TURTLEBOT_PROFILING" == "1" ] ; then
for proffile in /tmp/robot_fsm.prof* ; do
google-pprof --pdf ./install/lib/iam/robot_fsm $proffile > $proffile.pdf
done
for proffile in /tmp/sick_line_guidance_demo.prof* ; do
google-pprof --pdf ./install/lib/sick_line_guidance_demo/sick_line_guidance_demo_node $proffile > $proffile.pdf
done
fi
#
# Play video recorded by sick_line_guidance_demo
#
popd
cp ~/.ros/log/watchdog.log ./log
cp ~/.ros/log/robot_fsm.log ./log
cp ~/.ros/log/sick_line_guidance_demo_node.log ./log
if [ "$TURTLEBOT_PROFILING" == "1" ] ; then
cp /tmp/robot_fsm.prof*.pdf ./log
cp /tmp/sick_line_guidance_demo.prof*.pdf ./log
fi
if [ -f ~/.ros/sick_line_guidance_demo.avi ] ; then
mv ~/.ros/sick_line_guidance_demo.avi ./log/sick_line_guidance_demo.avi
echo -e "\nCreated sick_line_guidance_demo.avi:"
ls -al ./log/sick_line_guidance_demo.avi
echo -e "ffplay ./sick_line_guidance_demo.avi ; # Use right mouse button to seek forward/backward, 'p' for pause/resume"
ffplay ./log/sick_line_guidance_demo.avi
echo -e "\n"
fi
ls -al ./log/*

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

@@ -0,0 +1,10 @@
#!/bin/bash
export TURTLEBOT_SIMULATION=1
export TURTLEBOT_OLS_SIMULATION=1
export TURTLEBOT_VISUALIZE=2
export TURTLEBOT_START_WATCHDOG_AFTER=-1 # 60
export TURTLEBOT_PROFILING=0
export TURTLEBOT_STOP_AT_START=0
./rungeneric.bash

10
Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot.bash Executable file
View File

@@ -0,0 +1,10 @@
#!/bin/bash
export TURTLEBOT_SIMULATION=0
export TURTLEBOT_OLS_SIMULATION=0
export TURTLEBOT_VISUALIZE=0
export TURTLEBOT_START_WATCHDOG_AFTER=0
export TURTLEBOT_PROFILING=0
export TURTLEBOT_STOP_AT_START=0
./rungeneric.bash

10
Devices/Packages/sick_line_guidance/turtlebotDemo/test/scripts/runturtlebot_olssimu.bash Executable file
View File

@@ -0,0 +1,10 @@
#!/bin/bash
export TURTLEBOT_SIMULATION=0
export TURTLEBOT_OLS_SIMULATION=1
export TURTLEBOT_VISUALIZE=0
export TURTLEBOT_START_WATCHDOG_AFTER=60
export TURTLEBOT_PROFILING=0
export TURTLEBOT_STOP_AT_START=0
./rungeneric.bash

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