diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..208c88a --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,17 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/opt/ros/humble/include/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "cStandard": "c17", + "cppStandard": "gnu++17", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/.vscode/settings.json b/chapt10/chapt10_ws/.vscode/settings.json new file mode 100644 index 0000000..f6e40f6 --- /dev/null +++ b/chapt10/chapt10_ws/.vscode/settings.json @@ -0,0 +1,70 @@ +{ + "files.associations": { + "array": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp" + } +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/pub5_sub5_2M.txt b/chapt10/chapt10_ws/pub5_sub5_2M.txt new file mode 100644 index 0000000..b5ddf61 --- /dev/null +++ b/chapt10/chapt10_ws/pub5_sub5_2M.txt @@ -0,0 +1,2729 @@ +1 +1 +0 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +0 +0 +1 +1 +1 +1 +0 +0 +0 +0 +0 +0 +1 +0 +1 +1 +0 +1 +1 +0 +1 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +1 +1 +0 +1 +1 +0 +1 +1 +0 +1 +1 +0 +1 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +0 +1 +1 +1 +1 +1 +0 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +2 +1 +1 +1 +1 +1 +1 +0 +0 +1 +0 +0 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +0 +1 +1 +1 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +2 +4 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +0 +1 +1 +1 +1 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +1 +0 +1 +0 +1 +1 +0 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +0 +1 +1 +1 +1 +1 +1 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +1 +1 +1 +0 +2 +1 +1 +0 +0 +0 +0 +2 +1 +1 +2 +0 +1 +2 +2 +1 +1 +0 +0 +0 +0 +1 +0 +0 +1 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +2 +1 +1 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +0 +1 +2 +2 +1 +1 +1 +4 +1 +1 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +2 +1 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +2 +0 +0 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +2 +1 +0 +1 +0 +1 +2 +1 +3 +2 +0 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +0 +0 +0 +0 +0 +0 +0 +1 +2 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +0 +0 +3 +0 +0 +0 +0 +1 +1 +0 +1 +0 +1 +0 +1 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +1 +1 +0 +0 +0 +1 +0 +1 +1 +1 +1 +0 +1 +0 +1 +0 +1 +1 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +2 +1 +1 +0 +1 +2 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +1 +1 +0 +0 +0 +1 +0 +1 +0 +1 +0 +0 +0 +0 +1 +0 +0 +0 +0 +1 +0 +0 +0 +1 +0 +0 +0 +0 +1 +0 +0 +1 +0 +0 +1 +0 +0 +0 +2 +0 +1 +0 +0 +0 +0 +1 +0 +0 +0 +0 +1 +0 +0 +0 +1 +1 +1 +1 +2 +0 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +0 +2 +1 +1 +1 +1 +1 +1 +1 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +1 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +1 +0 +1 +0 +0 +0 +0 +2 +0 +0 +1 +2 +2 +1 +1 +1 +1 +1 +0 +2 +1 +1 +3 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +2 +2 +2 +2 +2 +2 +0 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +4 +4 +3 +4 +3 +4 +4 +3 +4 +4 +2 +3 +3 +2 +2 +2 +4 +3 +3 +3 +2 +3 +3 +3 +3 +6 +3 +3 +3 +3 +3 +3 +3 +3 +2 +2 +2 +3 +2 +2 +2 +3 +2 +3 +3 +3 +3 +3 +3 +3 +3 +3 +3 +3 +3 +7 +3 +3 +3 +3 +6 +5 +3 +3 +3 +3 +3 +4 +7 +4 +3 +2 +1 +0 +0 +0 +1 +0 +0 +1 +0 +0 +0 +0 +0 +1 +1 +1 +0 +1 +1 +0 +1 +0 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +1 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +1 +0 +0 +0 +1 +1 +1 +0 +0 +0 +1 +1 +3 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +1 +1 +1 +0 +0 +0 +4 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +2 +1 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +0 +1 +0 +0 +0 +0 +0 +0 +0 +1 +1 +1 +0 +0 +0 +0 +0 +1 +0 +1 +0 +0 +1 +1 +1 +0 +1 +0 +0 +1 +1 +0 +1 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +0 +0 +0 +1 +0 +0 +1 +1 +0 +1 +1 +0 +0 +1 +0 +1 +1 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +0 +1 +1 +1 +0 +1 +1 +1 +1 +0 +1 +1 +1 +1 +0 +0 +0 +0 +1 +0 +0 +0 +1 +1 +1 +1 +0 +1 +0 +0 +1 +1 +1 +1 +1 +1 +0 +1 +1 +0 +0 +1 +0 +0 +0 +0 +1 +1 +1 +1 +2 +1 +1 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +2 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +0 +0 +1 +1 +1 +0 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +2 +1 +3 +1 +1 +1 +0 +1 +1 +0 +0 +1 +1 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +0 +1 +1 +1 +1 +0 +1 +1 +0 +1 +1 +1 +1 +1 +1 +1 +1 +0 +1 +1 +1 +209040809 +209042537 +209044355 +209046146 +209047966 +209049870 +209051509 +209053381 +209055177 +209057057 +209058904 +209060530 +209062708 +209064646 +209066060 +209067873 +209069610 +209071567 +209073352 +209075115 +209076943 +209078563 +209080340 +209082169 +209083978 +209085769 +209087618 +209089358 +209091181 +209093167 +209094746 +209096553 +209098503 +209100175 +209101971 +209103762 +209105507 +209107480 +209109191 +209110954 +209112744 +209114566 +209116367 +209118230 +209119920 +209121728 +209123542 +209125439 +209127226 +209128934 +209130723 +209132548 +209134331 +209136191 +209138133 +209139839 +209141588 +209143467 +209145355 +209147337 +209148844 +209150629 +209152451 +209154772 +209157003 +209158232 +209159775 +209161503 +209163232 +209165000 +209166746 +209168669 +209170360 +209172165 +209174419 +209176084 +209177772 +209179583 +209181314 +209183096 +209184750 +209186605 +209188377 +209190236 +209192288 +209193862 +209195583 +209197384 +209199249 +209201068 +209202889 +209204973 +209206450 +209208230 +209210148 +209212206 +209213581 +209215457 +209217219 +209219546 +209220809 +209223324 +209224605 +209226197 +209228072 +209229782 +209231623 +209233579 +209235360 +209237238 +209239295 +209240829 +209242395 +209244244 +209246472 +209248763 +209250356 +73 +57 +51 +59 +102 +58 +65 +65 +55 +58 +58 +67 +63 +68 +61 +88 +71 +63 +73 +59 +60 +92 +55 +64 +62 +61 +76 +62 +79 +78 +72 +92 +82 +63 +65 +61 +62 +65 +65 +54 +104 +74 +69 +52 +58 +57 +92 +67 +74 +63 +65 +53 +58 +56 +64 +91 +103 +107 +73 +68 +56 +81 +69 +88 +75 +62 +55 +46 +53 +57 +65 +60 +54 +66 +56 +47 +47 +47 +47 +53 +53 +51 +59 +66 +76 +46 +84 +76 +191 +50 +48 +46 +51 +60 +53 +78 +56 +48 +56 +50 +61 +76 +76 +53 +45 +56 +56 +72 +50 +52 +61 +58 +54 +59 +86 +68 +69 +58 +54 +50 +58 +96 +159 +48 +53 +63 +59 +64 +54 +59 +53 +47 +56 +56 +66 +47 +52 +48 +59 +47 +52 +53 +46 +45 +85 +71 +75 +55 +67 +85 +86 +73 +59 +58 +56 +57 +54 +68 +119 +102 +146 +66 +111 +61 +60 +69 +58 +57 +89 +56 +60 +63 +60 +59 +52 +64 +128 +104 +57 +56 +83 +55 +46 +30 +37 +28 +25 +39 +44 +33 +28 diff --git a/chapt10/chapt10_ws/shm.xml b/chapt10/chapt10_ws/shm.xml new file mode 100644 index 0000000..b9e53be --- /dev/null +++ b/chapt10/chapt10_ws/shm.xml @@ -0,0 +1,24 @@ + + + + + + + SYNCHRONOUS + + + AUTOMATIC + + + DYNAMIC + + + + + + AUTOMATIC + + + DYNAMIC + + \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_compose/CMakeLists.txt b/chapt10/chapt10_ws/src/learn_compose/CMakeLists.txt new file mode 100644 index 0000000..293534a --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_compose/CMakeLists.txt @@ -0,0 +1,56 @@ +cmake_minimum_required(VERSION 3.8) +project(learn_compose) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +include_directories(include) +add_executable(intra_process_pubsub + src/intra_process_pubsub.cpp + src/talker.cpp + src/listener.cpp +) +ament_target_dependencies(intra_process_pubsub std_msgs rclcpp rclcpp_components) + +install(TARGETS intra_process_pubsub + DESTINATION lib/${PROJECT_NAME}) + + +find_package(rclcpp_components REQUIRED) +add_library(talker_component SHARED src/talker.cpp) +ament_target_dependencies(talker_component "std_msgs" "rclcpp" "rclcpp_components") +rclcpp_components_register_nodes(talker_component "learn_compose::Talker") + +add_library(listener_component SHARED src/listener.cpp) +ament_target_dependencies(listener_component "std_msgs" "rclcpp" "rclcpp_components") +rclcpp_components_register_nodes(listener_component "learn_compose::Listener") + +install(TARGETS talker_component listener_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_package() diff --git a/chapt10/chapt10_ws/src/learn_compose/include/learn_compose/listener.hpp b/chapt10/chapt10_ws/src/learn_compose/include/learn_compose/listener.hpp new file mode 100644 index 0000000..8692f60 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_compose/include/learn_compose/listener.hpp @@ -0,0 +1,19 @@ +#ifndef LEARN_COMPOSE__LISTENER_COMPONENT_HPP_ +#define LEARN_COMPOSE__LISTENER_COMPONENT_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +namespace learn_compose { + +class Listener : public rclcpp::Node { +public: + explicit Listener(const rclcpp::NodeOptions &options); + +private: + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace learn_compose + +#endif // LEARN_COMPOSE__LISTENER_COMPONENT_HPP_ \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_compose/include/learn_compose/talker.hpp b/chapt10/chapt10_ws/src/learn_compose/include/learn_compose/talker.hpp new file mode 100644 index 0000000..60b3239 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_compose/include/learn_compose/talker.hpp @@ -0,0 +1,21 @@ +#ifndef LEARN_COMPOSE__TALKER_COMPONENT_HPP_ +#define LEARN_COMPOSE__TALKER_COMPONENT_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +namespace learn_compose { + +class Talker : public rclcpp::Node { +public: + explicit Talker(const rclcpp::NodeOptions &options); + +private: + int32_t count_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace learn_compose + +#endif // LEARN_COMPOSE__TALKER_COMPONENT_HPP_ \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_compose/package.xml b/chapt10/chapt10_ws/src/learn_compose/package.xml new file mode 100644 index 0000000..f97a616 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_compose/package.xml @@ -0,0 +1,18 @@ + + + + learn_compose + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt10/chapt10_ws/src/learn_compose/src/intra_process_pubsub.cpp b/chapt10/chapt10_ws/src/learn_compose/src/intra_process_pubsub.cpp new file mode 100644 index 0000000..af30db8 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_compose/src/intra_process_pubsub.cpp @@ -0,0 +1,21 @@ +#include "learn_compose/listener.hpp" +#include "learn_compose/talker.hpp" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + + rclcpp::NodeOptions options; // 创建节点选项 + options.use_intra_process_comms(true); // 使用进程内通信 + auto talker = std::make_shared(options); + auto listener = std::make_shared(options); + + executor.add_node(talker); + executor.add_node(listener); + executor.spin(); + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_compose/src/listener.cpp b/chapt10/chapt10_ws/src/learn_compose/src/listener.cpp new file mode 100644 index 0000000..fde1ef5 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_compose/src/listener.cpp @@ -0,0 +1,20 @@ +#include "learn_compose/listener.hpp" +#include + +namespace learn_compose { + +using namespace std::chrono_literals; + +Listener::Listener(const rclcpp::NodeOptions &options) + : Node("listener", options) { + sub_ = this->create_subscription( + "count", 10, [&](const std_msgs::msg::Int32::UniquePtr msg) { + RCLCPP_INFO(this->get_logger(), "收到数据:%d(0x%lX)", msg->data, + reinterpret_cast(msg.get())); + }); +} +} // namespace learn_compose + + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(learn_compose::Listener) \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_compose/src/talker.cpp b/chapt10/chapt10_ws/src/learn_compose/src/talker.cpp new file mode 100644 index 0000000..1596472 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_compose/src/talker.cpp @@ -0,0 +1,24 @@ +#include +#include "learn_compose/talker.hpp" + +namespace learn_compose { + +using namespace std::chrono_literals; + +Talker::Talker(const rclcpp::NodeOptions &options) : Node("talker", options) { + pub_ = this->create_publisher("count", 10); + auto callback = [&]() -> void { + std_msgs::msg::Int32::UniquePtr msg(new std_msgs::msg::Int32()); + msg->data = count_++; + RCLCPP_INFO(this->get_logger(), "发布数据:%d(0x%lX)", msg->data, + reinterpret_cast(msg.get())); + pub_->publish(std::move(msg)); + }; + timer_ = this->create_wall_timer(1s, callback); +} +} // namespace learn_compose + + + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(learn_compose::Talker) \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_dds_cpp/CMakeLists.txt b/chapt10/chapt10_ws/src/learn_dds_cpp/CMakeLists.txt new file mode 100644 index 0000000..adb278d --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_dds_cpp/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(learn_dds_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + + +add_executable(shm_pub src/shm_pub.cpp) +ament_target_dependencies(shm_pub + rclcpp std_msgs +) + +install(TARGETS shm_pub + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + + + +ament_package() diff --git a/chapt10/chapt10_ws/src/learn_dds_cpp/package.xml b/chapt10/chapt10_ws/src/learn_dds_cpp/package.xml new file mode 100644 index 0000000..cae164e --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_dds_cpp/package.xml @@ -0,0 +1,21 @@ + + + + learn_dds_cpp + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt10/chapt10_ws/src/learn_dds_cpp/src/shm_pub.cpp b/chapt10/chapt10_ws/src/learn_dds_cpp/src/shm_pub.cpp new file mode 100644 index 0000000..424d4b2 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_dds_cpp/src/shm_pub.cpp @@ -0,0 +1,30 @@ +#include "rclcpp/loaned_message.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +class LoanedMessagePublisher : public rclcpp::Node { +public: + LoanedMessagePublisher() : Node("loaned_message_publisher") { + publisher_ = + this->create_publisher("loaned_int_topic", 10); + timer_ = this->create_wall_timer(std::chrono::seconds(1), [&]() { + auto message = publisher_->borrow_loaned_message(); // 1.租借消息 + message.get().data = count_++; // 2.放入数据 + RCLCPP_INFO(this->get_logger(), "发布数据:%d", message.get().data); + publisher_->publish(std::move(message)); // 3.发布数据 + }); + } + +private: + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + int32_t count_{0}; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_executor_cpp/CMakeLists.txt b/chapt10/chapt10_ws/src/learn_executor_cpp/CMakeLists.txt new file mode 100644 index 0000000..2448b7f --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_cpp/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(learn_executor_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +find_package(rclcpp REQUIRED) +find_package(example_interfaces REQUIRED) +find_package(std_msgs REQUIRED) + +add_executable(learn_executor src/learn_executor.cpp) +ament_target_dependencies(learn_executor rclcpp example_interfaces std_msgs) + +install(TARGETS learn_executor + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/chapt10/chapt10_ws/src/learn_executor_cpp/package.xml b/chapt10/chapt10_ws/src/learn_executor_cpp/package.xml new file mode 100644 index 0000000..e2a5db1 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_cpp/package.xml @@ -0,0 +1,18 @@ + + + + learn_executor_cpp + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt10/chapt10_ws/src/learn_executor_cpp/src/learn_executor.cpp b/chapt10/chapt10_ws/src/learn_executor_cpp/src/learn_executor.cpp new file mode 100644 index 0000000..7883da9 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_cpp/src/learn_executor.cpp @@ -0,0 +1,62 @@ +#include "example_interfaces/srv/add_two_ints.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include + +class LearnExecutorNode : public rclcpp::Node { +public: + LearnExecutorNode() : Node("learn_executor") { + publisher_ = + this->create_publisher("string_topic", 10); + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + std::bind(&LearnExecutorNode::timer_callback, this)); + service_callback_group_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); // 互斥回调组 + service_ = this->create_service( + "add_two_ints", + std::bind(&LearnExecutorNode::add_two_ints_callback, this, + std::placeholders::_1, std::placeholders::_2), + rmw_qos_profile_services_default, service_callback_group_); + } + +private: + void timer_callback() { + auto msg = std_msgs::msg::String(); + msg.data = "话题发布：" + thread_info(); + RCLCPP_INFO(this->get_logger(), msg.data.c_str()); + publisher_->publish(msg); + } + + std::string thread_info() { + std::ostringstream thread_str; + thread_str << "线程ID：" << std::this_thread::get_id(); + return thread_str.str(); + } + + void add_two_ints_callback( + const std::shared_ptr + request, + std::shared_ptr response) { + RCLCPP_INFO(this->get_logger(), "服务开始处理：%s", thread_info().c_str()); + std::this_thread::sleep_for(std::chrono::seconds(10)); + response->sum = request->a + request->b; + RCLCPP_INFO(this->get_logger(), "服务处理完成：%s", thread_info().c_str()); + } + + rclcpp::Publisher::SharedPtr publisher_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Service::SharedPtr service_; + rclcpp::CallbackGroup::SharedPtr service_callback_group_; +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); +// auto executor = rclcpp::executors::SingleThreadedExecutor(); + auto executor = rclcpp::executors::MultiThreadedExecutor(); + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_executor_py/learn_executor_py/__init__.py b/chapt10/chapt10_ws/src/learn_executor_py/learn_executor_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_executor_py/learn_executor_py/learn_executor.py b/chapt10/chapt10_ws/src/learn_executor_py/learn_executor_py/learn_executor.py new file mode 100644 index 0000000..2b903bf --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_py/learn_executor_py/learn_executor.py @@ -0,0 +1,44 @@ +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup +from std_msgs.msg import String +from example_interfaces.srv import AddTwoInts +import threading +import time + + +class LearnExecutorNode(Node): + def __init__(self): + super().__init__('learn_executor') + self.publisher = self.create_publisher(String, 'string_topic', 10) + self.timer = self.create_timer(1.0, self.timer_callback) + my_callback_group = ReentrantCallbackGroup() + self.service = self.create_service( + AddTwoInts, 'add_two_ints', self.add_two_ints_callback, + callback_group=my_callback_group) + # self.service = self.create_service( + # AddTwoInts, 'add_two_ints', self.add_two_ints_callback) + + def timer_callback(self): + msg = String() + msg.data = f'话题发布，线程ID:{threading.get_ident()} 线程总数:{threading.active_count()}' + self.get_logger().info(msg.data) + self.publisher.publish(msg) + + def add_two_ints_callback(self, request: AddTwoInts.Request, response: AddTwoInts.Response): + self.get_logger().info(f'处理服务，线程ID:{threading.get_ident()}') + time.sleep(10) # 模拟处理延时 + response.sum = request.a + request.b + self.get_logger().info(f'处理完成，线程ID:{threading.get_ident()}') + return response + + +def main(args=None): + rclpy.init(args=args) + node = LearnExecutorNode() + # executor = SingleThreadedExecutor() + executor = MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + rclpy.shutdown() \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_executor_py/package.xml b/chapt10/chapt10_ws/src/learn_executor_py/package.xml new file mode 100644 index 0000000..fc2e166 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_py/package.xml @@ -0,0 +1,18 @@ + + + + learn_executor_py + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt10/chapt10_ws/src/learn_executor_py/resource/learn_executor_py b/chapt10/chapt10_ws/src/learn_executor_py/resource/learn_executor_py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_executor_py/setup.cfg b/chapt10/chapt10_ws/src/learn_executor_py/setup.cfg new file mode 100644 index 0000000..3c8ff06 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/learn_executor_py +[install] +install_scripts=$base/lib/learn_executor_py diff --git a/chapt10/chapt10_ws/src/learn_executor_py/setup.py b/chapt10/chapt10_ws/src/learn_executor_py/setup.py new file mode 100644 index 0000000..97702ea --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_py/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'learn_executor_py' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'learn_executor=learn_executor_py.learn_executor:main' + ], + }, +) diff --git a/chapt10/chapt10_ws/src/learn_executor_py/test/test_copyright.py b/chapt10/chapt10_ws/src/learn_executor_py/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt10/chapt10_ws/src/learn_executor_py/test/test_flake8.py b/chapt10/chapt10_ws/src/learn_executor_py/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt10/chapt10_ws/src/learn_executor_py/test/test_pep257.py b/chapt10/chapt10_ws/src/learn_executor_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_executor_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/CMakeLists.txt b/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/CMakeLists.txt new file mode 100644 index 0000000..b7d6530 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(learn_lifecyclenode_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +add_executable(learn_lifecyclenode src/learn_lifecyclenode.cpp) +ament_target_dependencies(learn_lifecyclenode + rclcpp rclcpp_lifecycle +) + +install(TARGETS learn_lifecyclenode + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/package.xml b/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/package.xml new file mode 100644 index 0000000..7b39c6c --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/package.xml @@ -0,0 +1,21 @@ + + + + learn_lifecyclenode_cpp + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_lifecycle + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/src/learn_lifecyclenode.cpp b/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/src/learn_lifecyclenode.cpp new file mode 100644 index 0000000..59f2c2f --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_cpp/src/learn_lifecyclenode.cpp @@ -0,0 +1,61 @@ +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +using CallbackReturn = + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +class LearnLifeCycleNode : public rclcpp_lifecycle::LifecycleNode { +public: + LearnLifeCycleNode() + : rclcpp_lifecycle::LifecycleNode("lifecyclenode") { + timer_period_ = 1.0; + timer_ = nullptr; + RCLCPP_INFO(get_logger(), "%s: 已创建", get_name()); + } + + CallbackReturn on_configure(const rclcpp_lifecycle::State &state) override { + (void)state; + timer_period_ = 1.0; + RCLCPP_INFO(get_logger(), "on_configure():配置周期 timer_period"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: + CallbackReturn::SUCCESS; + } + + CallbackReturn on_activate(const rclcpp_lifecycle::State &state) override { + (void)state; + timer_ = create_wall_timer( + std::chrono::seconds(static_cast(timer_period_)), + [this]() { RCLCPP_INFO(get_logger(), "定时器打印进行中..."); }); + RCLCPP_INFO(get_logger(), "on_activate():处理激活指令，创建定时器"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: + CallbackReturn::SUCCESS; + } + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State &state) override { + (void)state; + timer_.reset(); + RCLCPP_INFO(get_logger(), "on_deactivate():处理失活指令，停止定时器"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: + CallbackReturn::SUCCESS; + } + + CallbackReturn on_shutdown(const rclcpp_lifecycle::State &state) override { + (void)state; + timer_.reset(); + RCLCPP_INFO(get_logger(), "on_shutdown():处理关闭指令"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface:: + CallbackReturn::SUCCESS; + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + double timer_period_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/learn_lifecyclenode_py/__init__.py b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/learn_lifecyclenode_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/learn_lifecyclenode_py/learn_lifecyclenode.py b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/learn_lifecyclenode_py/learn_lifecyclenode.py new file mode 100644 index 0000000..dde44c6 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/learn_lifecyclenode_py/learn_lifecyclenode.py @@ -0,0 +1,49 @@ +import rclpy +from rclpy.lifecycle import LifecycleNode, TransitionCallbackReturn + +class LearnLifeCycleNode(LifecycleNode): + def __init__(self): + super().__init__('lifecyclenode') + self.timer_period = 0 + self.timer_ = None + self.get_logger().info(f'{self.get_name()}:已创建') + + def timer_callback(self): + self.get_logger().info('定时器打印进行中...') + + def on_configure(self, state): + self.timer_period = 1.0 # 设置定时器周期 + self.get_logger().info('on_configure():配置周期 timer_period') + return TransitionCallbackReturn.SUCCESS + + def on_activate(self, state): + self.timer_ = self.create_timer(self.timer_period, self.timer_callback) + self.get_logger().info('on_activate():处理激活指令，创建定时器') + return TransitionCallbackReturn.SUCCESS + + def on_deactivate(self, state): + self.destroy_timer(self.timer_) # 销毁定时器 + self.get_logger().info('on_deactivate():处理失活指令停止定时器') + return TransitionCallbackReturn.SUCCESS + + def on_cleanup(self, state): + self.timer_ = None + self.timer_period = 0 + self.get_logger().info('on_cleanup():处理清理指令') + return TransitionCallbackReturn.SUCCESS + + def on_shutdown(self, state): + # 定时器未销毁则销毁 + if self.timer_: self.destroy_timer(self.timer_) + self.get_logger().info('on_shutdown():处理关闭指令') + return TransitionCallbackReturn.SUCCESS + + def on_error(self, state): + # 直接调用父类处理 + return super().on_error(state) + +def main(): + rclpy.init() + node = LearnLifeCycleNode() + rclpy.spin(node) + rclpy.shutdown() \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/package.xml b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/package.xml new file mode 100644 index 0000000..d2d15a4 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/package.xml @@ -0,0 +1,18 @@ + + + + learn_lifecyclenode_py + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/resource/learn_lifecyclenode_py b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/resource/learn_lifecyclenode_py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/setup.cfg b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/setup.cfg new file mode 100644 index 0000000..fc53b44 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/learn_lifecyclenode_py +[install] +install_scripts=$base/lib/learn_lifecyclenode_py diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/setup.py b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/setup.py new file mode 100644 index 0000000..a6b35ca --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'learn_lifecyclenode_py' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'learn_lifecyclenode=learn_lifecyclenode_py.learn_lifecyclenode:main' + ], + }, +) diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_copyright.py b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_flake8.py b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_pep257.py b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_lifecyclenode_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt10/chapt10_ws/src/learn_message_filter_cpp/CMakeLists.txt b/chapt10/chapt10_ws/src/learn_message_filter_cpp/CMakeLists.txt new file mode 100644 index 0000000..0b92616 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_cpp/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(learn_message_filter_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(message_filters REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_executable(timesync_test src/timesync_test.cpp) +ament_target_dependencies(timesync_test + rclcpp message_filters nav_msgs sensor_msgs +) + +install(TARGETS timesync_test + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/chapt10/chapt10_ws/src/learn_message_filter_cpp/package.xml b/chapt10/chapt10_ws/src/learn_message_filter_cpp/package.xml new file mode 100644 index 0000000..e6df4b0 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_cpp/package.xml @@ -0,0 +1,23 @@ + + + + learn_message_filter_cpp + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + rclcpp + message_filters + nav_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt10/chapt10_ws/src/learn_message_filter_cpp/src/timesync_test.cpp b/chapt10/chapt10_ws/src/learn_message_filter_cpp/src/timesync_test.cpp new file mode 100644 index 0000000..8e0c9a8 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_cpp/src/timesync_test.cpp @@ -0,0 +1,70 @@ +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/sync_policies/exact_time.h" +#include "message_filters/sync_policies/latest_time.h" +#include "message_filters/time_synchronizer.h" +#include "nav_msgs/msg/odometry.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" + +using Imu = sensor_msgs::msg::Imu; +using Odometry = nav_msgs::msg::Odometry; +using namespace message_filters; + +// 同步策略：严格时间对齐策略 +// using MySyncPolicy = sync_policies::ExactTime; +// 同步策略：大约时间对齐策略 +// using MySyncPolicy = sync_policies::ApproximateTime; +// 同步策略：最新时间对齐策略 +using MySyncPolicy = sync_policies::LatestTime; + + +class TimeSyncTestNode : public rclcpp::Node { +public: + TimeSyncTestNode() : Node("sync_node") { + // 1.订阅 imu 话题并注册回调并打印时间戳 + imu_sub_ = std::make_shared>(this, "imu"); + imu_sub_->registerCallback( + [&](const Imu::SharedPtr &imu_msg) { + RCLCPP_INFO(get_logger(), "imu(%u,%u)", imu_msg->header.stamp.sec, + imu_msg->header.stamp.nanosec); + }); + // 2.订阅 odom 话题并注册回调函数打印时间戳 + odom_sub_ = std::make_shared>(this, "odom"); + odom_sub_->registerCallback( + [&](const Odometry::SharedPtr &odom_msg) { + RCLCPP_INFO(get_logger(), "odom(%u,%u)", odom_msg->header.stamp.sec, + odom_msg->header.stamp.nanosec); + }); + // 3.创建对应策略的同步器同步两个话题，并注册回调函数打印数据 + // synchronizer_ = std::make_shared>( + // MySyncPolicy(10), *imu_sub_, *odom_sub_); + + // 3.创建对应策略的同步器同步两个话题，并注册回调函数打印数据 + synchronizer_ = std::make_shared>( + MySyncPolicy(), *imu_sub_, *odom_sub_); + synchronizer_->registerCallback( + std::bind(&TimeSyncTestNode::result_callback, this, + std::placeholders::_1, std::placeholders::_2)); + } + +private: + void result_callback(const Imu::ConstSharedPtr imu_msg, + const Odometry::ConstSharedPtr odom_msg) { + RCLCPP_INFO(get_logger(), "imu(%u,%u),odom(%u,%u))", + imu_msg->header.stamp.sec, imu_msg->header.stamp.nanosec, + odom_msg->header.stamp.sec, odom_msg->header.stamp.nanosec); + } + + std::shared_ptr> imu_sub_; + std::shared_ptr> odom_sub_; + std::shared_ptr> synchronizer_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/learn_message_filter_py/__init__.py b/chapt10/chapt10_ws/src/learn_message_filter_py/learn_message_filter_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/learn_message_filter_py/timesync_test.py b/chapt10/chapt10_ws/src/learn_message_filter_py/learn_message_filter_py/timesync_test.py new file mode 100644 index 0000000..0d197cc --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_py/learn_message_filter_py/timesync_test.py @@ -0,0 +1,40 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Imu +from nav_msgs.msg import Odometry +from message_filters import Subscriber, ApproximateTimeSynchronizer + +class TimeSyncTestNode(Node): + def __init__(self): + super().__init__('sync_node') + # 1. 订阅 imu 话题并注册回调并打印时间戳 + self.imu_sub = Subscriber(self, Imu, 'imu') + self.imu_sub.registerCallback(self.imu_callback) + # 2. 订阅 odom 话题并注册回调函数打印时间戳 + self.odom_sub = Subscriber(self, Odometry, 'odom') + self.odom_sub.registerCallback(self.odom_callback) + # 3. 创建对应策略的同步器同步两个话题，并注册回调函数打印数据 + self.synchronizer = ApproximateTimeSynchronizer( + [self.imu_sub, self.odom_sub], 10, + slop=0.01, # slop 表示时间窗口单位为秒 + ) + self.synchronizer.registerCallback(self.result_callback) + + def imu_callback(self, imu_msg): + self.get_logger().info( + f'imu({imu_msg.header.stamp.sec},{imu_msg.header.stamp.nanosec})') + + def odom_callback(self, odom_msg): + self.get_logger().info( + f'odom({odom_msg.header.stamp.sec},{odom_msg.header.stamp.nanosec})') + + def result_callback(self, imu_msg, odom_msg): + self.get_logger().info( + f'imu({imu_msg.header.stamp.sec},{imu_msg.header.stamp.nanosec}),odom({odom_msg.header.stamp.sec},{odom_msg.header.stamp.nanosec})') + + +def main(args=None): + rclpy.init(args=args) + node = TimeSyncTestNode() + rclpy.spin(node) + rclpy.shutdown() \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/package.xml b/chapt10/chapt10_ws/src/learn_message_filter_py/package.xml new file mode 100644 index 0000000..3c35501 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_py/package.xml @@ -0,0 +1,18 @@ + + + + learn_message_filter_py + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/resource/learn_message_filter_py b/chapt10/chapt10_ws/src/learn_message_filter_py/resource/learn_message_filter_py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/setup.cfg b/chapt10/chapt10_ws/src/learn_message_filter_py/setup.cfg new file mode 100644 index 0000000..66a92c6 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/learn_message_filter_py +[install] +install_scripts=$base/lib/learn_message_filter_py diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/setup.py b/chapt10/chapt10_ws/src/learn_message_filter_py/setup.py new file mode 100644 index 0000000..8b4e350 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_py/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'learn_message_filter_py' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'timesync_test=learn_message_filter_py.timesync_test:main' + ], + }, +) diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_copyright.py b/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_flake8.py b/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_pep257.py b/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_message_filter_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt10/chapt10_ws/src/learn_qos_cpp/CMakeLists.txt b/chapt10/chapt10_ws/src/learn_qos_cpp/CMakeLists.txt new file mode 100644 index 0000000..700b9e2 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_cpp/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.8) +project(learn_qos_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) + +add_executable(reliability_test src/reliability_test.cpp) +ament_target_dependencies(reliability_test + rclcpp nav_msgs +) + +install(TARGETS reliability_test + DESTINATION lib/${PROJECT_NAME}) + +ament_package() diff --git a/chapt10/chapt10_ws/src/learn_qos_cpp/package.xml b/chapt10/chapt10_ws/src/learn_qos_cpp/package.xml new file mode 100644 index 0000000..60c3fa0 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_cpp/package.xml @@ -0,0 +1,18 @@ + + + + learn_qos_cpp + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt10/chapt10_ws/src/learn_qos_cpp/src/reliability_test.cpp b/chapt10/chapt10_ws/src/learn_qos_cpp/src/reliability_test.cpp new file mode 100644 index 0000000..b89f21b --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_cpp/src/reliability_test.cpp @@ -0,0 +1,56 @@ +#include +#include + +class OdomPublisherSubscriber : public rclcpp::Node +{ +public: + OdomPublisherSubscriber() : Node("odom_publisher_subscriber") + { + rclcpp::QoS qos_profile(10); // 队列深度为10 + qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); // 可靠性策略 + qos_profile.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); // 持久性策略 + qos_profile.history(RMW_QOS_POLICY_HISTORY_KEEP_LAST); // 历史记录策略 + qos_profile.deadline(rclcpp::Duration(1, 0)); // 截止时间为1秒 + + odom_publisher_ = this->create_publisher( + "odom", qos_profile); + + // // 创建发布者并设置QoS为sensor + // odom_publisher_ = this->create_publisher( + // "odom", rclcpp::SensorDataQoS()); + + // 创建订阅者（默认QoS配置）队列深度设置为 5 + // odom_subscription_ = this->create_subscription( + // "odom", 5, + // [this](const nav_msgs::msg::Odometry::SharedPtr msg) { + // (void)msg; + // RCLCPP_INFO(this->get_logger(), "收到里程计消息"); + // }); + + odom_subscription_ = this->create_subscription( + "odom", rclcpp::SensorDataQoS(), + [this](const nav_msgs::msg::Odometry::SharedPtr msg) + { + (void)msg; + RCLCPP_INFO(this->get_logger(), "收到里程计消息"); + }); + + // 创建一个1秒的定时器，并指定回调函数 + timer_ = this->create_wall_timer(std::chrono::seconds(1), [this]() + { odom_publisher_->publish(nav_msgs::msg::Odometry()); }); + } + +private: + rclcpp::Publisher::SharedPtr odom_publisher_; + rclcpp::Subscription::SharedPtr odom_subscription_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto odom_node = std::make_shared(); + rclcpp::spin(odom_node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_qos_py/learn_qos_py/__init__.py b/chapt10/chapt10_ws/src/learn_qos_py/learn_qos_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_qos_py/learn_qos_py/reliability_test.py b/chapt10/chapt10_ws/src/learn_qos_py/learn_qos_py/reliability_test.py new file mode 100644 index 0000000..888daa6 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_py/learn_qos_py/reliability_test.py @@ -0,0 +1,41 @@ +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from rclpy import qos +from rclpy.duration import Duration + + + +class OdomPublisherSubscriber(Node): + + def __init__(self): + super().__init__('odom_publisher_subscriber') + # 创建发布者并设置QoS为sensor + qos_profile = qos.QoSProfile( + depth=10, # 队列深度 + reliability=qos.ReliabilityPolicy.BEST_EFFORT, # 可靠性 + durability=qos.DurabilityPolicy.TRANSIENT_LOCAL, # 持久性 + history=qos.HistoryPolicy.KEEP_LAST, # 历史记录策略 + deadline=Duration(seconds=1.0, nanoseconds=0), + ) + # 在订阅和发布中使用 + self.odom_publisher = self.create_publisher(Odometry, 'odom', qos_profile) + # 创建订阅者（默认QoS配置）队列深度设置为 5 + self.odom_subscriber = self.create_subscription( + Odometry, 'odom', self.odom_callback,qos.qos_profile_sensor_data) + # 创建一个1秒的定时器，并指定回调函数 + self.timer = self.create_timer(1.0, self.timer_callback) + + def odom_callback(self, msg): + self.get_logger().info('收到里程计消息') + + def timer_callback(self): + odom_msg = Odometry() # 创建一个Odometry消息 + self.odom_publisher.publish(odom_msg) # 发布消息 + + +def main(args=None): + rclpy.init(args=args) + odom_node = OdomPublisherSubscriber() + rclpy.spin(odom_node) + rclpy.shutdown() \ No newline at end of file diff --git a/chapt10/chapt10_ws/src/learn_qos_py/package.xml b/chapt10/chapt10_ws/src/learn_qos_py/package.xml new file mode 100644 index 0000000..fe21c03 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_py/package.xml @@ -0,0 +1,18 @@ + + + + learn_qos_py + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt10/chapt10_ws/src/learn_qos_py/resource/learn_qos_py b/chapt10/chapt10_ws/src/learn_qos_py/resource/learn_qos_py new file mode 100644 index 0000000..e69de29 diff --git a/chapt10/chapt10_ws/src/learn_qos_py/setup.cfg b/chapt10/chapt10_ws/src/learn_qos_py/setup.cfg new file mode 100644 index 0000000..6bb3d28 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/learn_qos_py +[install] +install_scripts=$base/lib/learn_qos_py diff --git a/chapt10/chapt10_ws/src/learn_qos_py/setup.py b/chapt10/chapt10_ws/src/learn_qos_py/setup.py new file mode 100644 index 0000000..bb3aa60 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_py/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'learn_qos_py' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'reliability_test=learn_qos_py.reliability_test:main' + ], + }, +) diff --git a/chapt10/chapt10_ws/src/learn_qos_py/test/test_copyright.py b/chapt10/chapt10_ws/src/learn_qos_py/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt10/chapt10_ws/src/learn_qos_py/test/test_flake8.py b/chapt10/chapt10_ws/src/learn_qos_py/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt10/chapt10_ws/src/learn_qos_py/test/test_pep257.py b/chapt10/chapt10_ws/src/learn_qos_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt10/chapt10_ws/src/learn_qos_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt10/chapt10_ws/topic_sub_limit.xml b/chapt10/chapt10_ws/topic_sub_limit.xml new file mode 100644 index 0000000..a19be44 --- /dev/null +++ b/chapt10/chapt10_ws/topic_sub_limit.xml @@ -0,0 +1,23 @@ + + + + + + DYNAMIC + + + + + DYNAMIC + + + + + DYNAMIC + + 0 + 1 + 1 + + + \ No newline at end of file diff --git a/chapt10/rosbag2_message_filter/metadata.yaml b/chapt10/rosbag2_message_filter/metadata.yaml new file mode 100644 index 0000000..3bb95c9 --- /dev/null +++ b/chapt10/rosbag2_message_filter/metadata.yaml @@ -0,0 +1,32 @@ +rosbag2_bagfile_information: + version: 5 + storage_identifier: sqlite3 + duration: + nanoseconds: 221085265896 + starting_time: + nanoseconds_since_epoch: 1697985428618130944 + message_count: 33156 + topics_with_message_count: + - topic_metadata: + name: /imu + type: sensor_msgs/msg/Imu + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 22104 + - topic_metadata: + name: /odom + type: nav_msgs/msg/Odometry + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + message_count: 11052 + compression_format: "" + compression_mode: "" + relative_file_paths: + - rosbag2_message_filter.db3 + files: + - path: rosbag2_message_filter.db3 + starting_time: + nanoseconds_since_epoch: 1697985428618130944 + duration: + nanoseconds: 221085265896 + message_count: 33156 \ No newline at end of file diff --git a/chapt10/rosbag2_message_filter/rosbag2_message_filter.db3 b/chapt10/rosbag2_message_filter/rosbag2_message_filter.db3 new file mode 100644 index 0000000..7629fa8 Binary files /dev/null and b/chapt10/rosbag2_message_filter/rosbag2_message_filter.db3 differ diff --git a/chapt7/chapt7_ws/README.md b/chapt7/chapt7_ws/README.md new file mode 100644 index 0000000..ec1ddee --- /dev/null +++ b/chapt7/chapt7_ws/README.md @@ -0,0 +1,83 @@ +# 基于 ROS 2 和 Navigation 2 自动巡检机器人 + +## 1.项目介绍 + +本项目基于 ROS 2 和 Navigation 2 设计了一个自动巡检机器人仿真功能。 + +该巡检机器人要能够在不同的目标点之间进行循环移动，每到达一个目标点后首先通过语音播放到达的目标点信息，接着通过摄像头采集一张实时的图像并保存到本地。 + +各功能包功能如下： +- fishbot_description 机器人描述文件，包含仿真相关配置 +- fishbot_navigation2 机器人导航配置文件 +- fishbot_application 机器人导航应用 Python 代码 +- fishbot_application_cpp 机器人导航应用 C++ 代码 +- autopatrol_interfaces 自动巡检相关接口 +- autopatrol_robot 自动巡检实现功能包 + +## 2.使用方法 + +本项目开发平台信息如下： + +- 系统版本： Ubunt22.04 +- ROS 版本：ROS 2 Humble + +### 2.1安装 + +本项目建图采用 slam-toolbox，导航采用 Navigation 2 ,仿真采用 Gazebo，运动控制采用 ros2-control 实现，构建之前请先安装依赖，指令如下： + +1. 安装 SLAM 和 Navigation 2 + +``` +sudo apt install ros-$ROS_DISTRO-nav2-bringup ros-$ROS_DISTRO-slam-toolbox +``` + +2. 安装仿真相关功能包 + +``` +sudo apt install ros-$ROS_DISTRO-robot-state-publisher ros-$ROS_DISTRO-joint-state-publisher ros-$ROS_DISTRO-gazebo-ros-pkgs ros-$ROS_DISTRO-ros2-controllers ros-$ROS_DISTRO-xacro +``` + +3. 安装语音合成和图像相关功能包 + +``` +sudo apt install python3-pip -y +sudo apt install espeak-ng -y +sudo pip3 install espeakng +sudo apt install ros-$ROS_DISTRO-tf-transformations +sudo pip3 install transforms3d +``` + +### 2.2运行 + +安装完成依赖后，可以使用 colcon 工具进行构建和运行。 + +构建功能包 + +``` +colcon build +``` + +运行仿真 + +``` +source install/setup.bash +ros2 launch fishbot_description gazebo.launch.py +``` + +运行导航 + +``` +source install/setup.bash +ros2 launch fishbot_navigation2 navigation2.launch.py +``` + +运行自动巡检 + +``` +source install/setup.bash +ros2 launch autopatrol_robot autopatrol.launch.py +``` + +## 3.作者 + +- [fishros](https://github.com/fishros) \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/autopatrol_interfaces/CMakeLists.txt b/chapt7/chapt7_ws/src/autopatrol_interfaces/CMakeLists.txt new file mode 100644 index 0000000..b2ed6b4 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_interfaces/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autopatrol_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rosidl_default_generators REQUIRED) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SpeachText.srv" +) +ament_package() diff --git a/chapt7/chapt7_ws/src/autopatrol_interfaces/package.xml b/chapt7/chapt7_ws/src/autopatrol_interfaces/package.xml new file mode 100644 index 0000000..ccdf647 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_interfaces/package.xml @@ -0,0 +1,18 @@ + + + + autopatrol_interfaces + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_interface_packages + + ament_cmake + + diff --git a/chapt7/chapt7_ws/src/autopatrol_interfaces/srv/SpeachText.srv b/chapt7/chapt7_ws/src/autopatrol_interfaces/srv/SpeachText.srv new file mode 100644 index 0000000..440fa28 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_interfaces/srv/SpeachText.srv @@ -0,0 +1,3 @@ +string text # 合成文字 +--- +bool result # 合成结果 \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/__init__.py b/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/patrol_node.py b/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/patrol_node.py new file mode 100644 index 0000000..e1e3f1c --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/patrol_node.py @@ -0,0 +1,173 @@ +import rclpy +from geometry_msgs.msg import PoseStamped, Pose +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from tf2_ros import TransformListener, Buffer +from tf_transformations import euler_from_quaternion, quaternion_from_euler +from rclpy.duration import Duration +# 添加服务接口 +from autopatrol_interfaces.srv import SpeachText +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +import cv2 + +class PatrolNode(BasicNavigator): + def __init__(self, node_name='patrol_node'): + super().__init__(node_name) + # 导航相关定义 + self.declare_parameter('initial_point', [0.0, 0.0, 0.0]) + self.declare_parameter('target_points', [0.0, 0.0, 0.0, 1.0, 1.0, 1.57]) + self.initial_point_ = self.get_parameter('initial_point').value + self.target_points_ = self.get_parameter('target_points').value + # 实时位置获取 TF 相关定义 + self.buffer_ = Buffer() + self.listener_ = TransformListener(self.buffer_, self) + self.speach_client_ = self.create_client(SpeachText, 'speech_text') + + # 订阅与保存图像相关定义 + self.declare_parameter('image_save_path', '') + self.image_save_path = self.get_parameter('image_save_path').value + self.bridge = CvBridge() + self.latest_image = None + self.subscription_image = self.create_subscription( + Image, '/camera_sensor/image_raw', self.image_callback, 10) + + def image_callback(self, msg): + """ + 将最新的消息放到 latest_image 中 + """ + self.latest_image = msg + + def record_image(self): + """ + 记录图像 + """ + if self.latest_image is not None: + pose = self.get_current_pose() + cv_image = self.bridge.imgmsg_to_cv2(self.latest_image) + cv2.imwrite(f'{self.image_save_path}image_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png', cv_image) + + + def speach_text(self, text): + """ + 调用服务播放语音 + """ + while not self.speach_client_.wait_for_service(timeout_sec=1.0): + self.get_logger().info('语合成服务未上线，等待中。。。') + + request = SpeachText.Request() + request.text = text + future = self.speach_client_.call_async(request) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + result = future.result().result + if result: + self.get_logger().info(f'语音合成成功：{text}') + else: + self.get_logger().warn(f'语音合成失败：{text}') + else: + self.get_logger().warn('语音合成服务请求失败') + + def get_pose_by_xyyaw(self, x, y, yaw): + """ + 通过 x,y,yaw 合成 PoseStamped + """ + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.pose.position.x = x + pose.pose.position.y = y + rotation_quat = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation.x = rotation_quat[0] + pose.pose.orientation.y = rotation_quat[1] + pose.pose.orientation.z = rotation_quat[2] + pose.pose.orientation.w = rotation_quat[3] + return pose + + def init_robot_pose(self): + """ + 初始化机器人位姿 + """ + # 从参数获取初始化点 + self.initial_point_ = self.get_parameter('initial_point').value + # 合成位姿并进行初始化 + self.setInitialPose(self.get_pose_by_xyyaw( + self.initial_point_[0], self.initial_point_[1], self.initial_point_[2])) + # 等待直到导航激活 + self.waitUntilNav2Active() + + def get_target_points(self): + """ + 通过参数值获取目标点集合 + """ + points = [] + self.target_points_ = self.get_parameter('target_points').value + for index in range(int(len(self.target_points_)/3)): + x = self.target_points_[index*3] + y = self.target_points_[index*3+1] + yaw = self.target_points_[index*3+2] + points.append([x, y, yaw]) + self.get_logger().info(f'获取到目标点: {index}->({x},{y},{yaw})') + return points + + def nav_to_pose(self, target_pose): + """ + 导航到指定位姿 + """ + self.waitUntilNav2Active() + result = self.goToPose(target_pose) + while not self.isTaskComplete(): + feedback = self.getFeedback() + if feedback: + self.get_logger().info(f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达') + # 最终结果判断 + result = self.getResult() + if result == TaskResult.SUCCEEDED: + self.get_logger().info('导航结果：成功') + elif result == TaskResult.CANCELED: + self.get_logger().warn('导航结果：被取消') + elif result == TaskResult.FAILED: + self.get_logger().error('导航结果：失败') + else: + self.get_logger().error('导航结果：返回状态无效') + + def get_current_pose(self): + """ + 通过TF获取当前位姿 + """ + while rclpy.ok(): + try: + tf = self.buffer_.lookup_transform( + 'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1)) + transform = tf.transform + rotation_euler = euler_from_quaternion([ + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w + ]) + self.get_logger().info( + f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}') + return transform + except Exception as e: + self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}') + +def main(): + rclpy.init() + patrol = PatrolNode() + patrol.speach_text(text='正在初始化位置') + patrol.init_robot_pose() + patrol.speach_text(text='位置初始化完成') + + while rclpy.ok(): + for point in patrol.get_target_points(): + x, y, yaw = point[0], point[1], point[2] + # 导航到目标点 + target_pose = patrol.get_pose_by_xyyaw(x, y, yaw) + patrol.speach_text(text=f'准备前往目标点{x},{y}') + patrol.nav_to_pose(target_pose) + patrol.speach_text(text=f"已到达目标点{x},{y},准备记录图像") + patrol.record_image() + patrol.speach_text(text=f"图像记录完成") + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/speaker.py b/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/speaker.py new file mode 100644 index 0000000..32e7269 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/autopatrol_robot/speaker.py @@ -0,0 +1,26 @@ +import rclpy +from rclpy.node import Node +from autopatrol_interfaces.srv import SpeachText +import espeakng + +class Speaker(Node): + def __init__(self, node_name): + super().__init__(node_name) + self.speech_service = self.create_service( + SpeachText, 'speech_text', self.speak_text_callback) + self.speaker = espeakng.Speaker() + self.speaker.voice = 'zh' + + def speak_text_callback(self, request, response): + self.get_logger().info('正在朗读 %s' % request.text) + self.speaker.say(request.text) + self.speaker.wait() + response.result = True + return response + + +def main(args=None): + rclpy.init(args=args) + node = Speaker('speaker') + rclpy.spin(node) + rclpy.shutdown() \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/config/patrol_config.yaml b/chapt7/chapt7_ws/src/autopatrol_robot/config/patrol_config.yaml new file mode 100644 index 0000000..cfef5b3 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/config/patrol_config.yaml @@ -0,0 +1,10 @@ +patrol_node: + ros__parameters: + initial_point: [0.0, 0.0, 0.0] + target_points: [ + 0.0,0.0,0.0, + 1.0,2.0,3.14, + -4.5,1.5,1.57, + -8.0,-5.0,1.57, + 1.0,-5.0,3.14, + ] \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/launch/autopatrol.launch.py b/chapt7/chapt7_ws/src/autopatrol_robot/launch/autopatrol.launch.py new file mode 100644 index 0000000..819362b --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/launch/autopatrol.launch.py @@ -0,0 +1,28 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # 获取与拼接默认路径 + autopatrol_robot_dir = get_package_share_directory( + 'autopatrol_robot') + patrol_config_path = os.path.join( + autopatrol_robot_dir, 'config', 'patrol_config.yaml') + + action_node_turtle_control = launch_ros.actions.Node( + package='autopatrol_robot', + executable='patrol_node', + parameters=[patrol_config_path] + ) + action_node_patrol_client = launch_ros.actions.Node( + package='autopatrol_robot', + executable='speaker', + ) + + return launch.LaunchDescription([ + action_node_turtle_control, + action_node_patrol_client, + ]) \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/package.xml b/chapt7/chapt7_ws/src/autopatrol_robot/package.xml new file mode 100644 index 0000000..aa36510 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/package.xml @@ -0,0 +1,18 @@ + + + + autopatrol_robot + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/resource/autopatrol_robot b/chapt7/chapt7_ws/src/autopatrol_robot/resource/autopatrol_robot new file mode 100644 index 0000000..e69de29 diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/setup.cfg b/chapt7/chapt7_ws/src/autopatrol_robot/setup.cfg new file mode 100644 index 0000000..60e9700 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/autopatrol_robot +[install] +install_scripts=$base/lib/autopatrol_robot diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/setup.py b/chapt7/chapt7_ws/src/autopatrol_robot/setup.py new file mode 100644 index 0000000..8b889e3 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +from glob import glob +package_name = 'autopatrol_robot' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name+'/config', ['config/patrol_config.yaml']), + ('share/' + package_name+'/launch', glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'patrol_node=autopatrol_robot.patrol_node:main', + 'speaker=autopatrol_robot.speaker:main', + ], + }, +) diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/test/test_copyright.py b/chapt7/chapt7_ws/src/autopatrol_robot/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/test/test_flake8.py b/chapt7/chapt7_ws/src/autopatrol_robot/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt7/chapt7_ws/src/autopatrol_robot/test/test_pep257.py b/chapt7/chapt7_ws/src/autopatrol_robot/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt7/chapt7_ws/src/autopatrol_robot/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/__init__.py b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/get_robot_pose.py b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/get_robot_pose.py new file mode 100644 index 0000000..fddcd8e --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/get_robot_pose.py @@ -0,0 +1,40 @@ +import rclpy +from rclpy.node import Node +from tf2_ros import TransformListener, Buffer +from tf_transformations import euler_from_quaternion + + +class TFListener(Node): + + def __init__(self): + super().__init__('tf2_listener') + self.buffer = Buffer() + self.listener = TransformListener(self.buffer, self) + self.timer = self.create_timer(1, self.get_transform) + + def get_transform(self): + try: + tf = self.buffer.lookup_transform( + 'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1)) + transform = tf.transform + rotation_euler = euler_from_quaternion([ + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w + ]) + self.get_logger().info( + f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}') + except Exception as e: + self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}') + + +def main(): + rclpy.init() + node = TFListener() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/init_robot_pose.py b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/init_robot_pose.py new file mode 100644 index 0000000..8ed9029 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/init_robot_pose.py @@ -0,0 +1,21 @@ +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + + +def main(): + rclpy.init() + navigator = BasicNavigator() + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.w = 1.0 + navigator.setInitialPose(initial_pose) + navigator.waitUntilNav2Active() + rclpy.spin(navigator) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/nav_to_pose.py b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/nav_to_pose.py new file mode 100644 index 0000000..45ff081 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/nav_to_pose.py @@ -0,0 +1,40 @@ +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + + +def main(): + rclpy.init() + navigator = BasicNavigator() + # 等待导航启动完成 + navigator.waitUntilNav2Active() + # 设置目标点坐标 + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 1.0 + goal_pose.pose.position.y = 1.0 + goal_pose.pose.orientation.w = 1.0 + # 发送目标接收反馈结果 + navigator.goToPose(goal_pose) + while not navigator.isTaskComplete(): + feedback = navigator.getFeedback() + navigator.get_logger().info( + f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达') + # 超时自动取消 + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelTask() + # 最终结果判断 + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + navigator.get_logger().info('导航结果：成功') + elif result == TaskResult.CANCELED: + navigator.get_logger().warn('导航结果：被取消') + elif result == TaskResult.FAILED: + navigator.get_logger().error('导航结果：失败') + else: + navigator.get_logger().error('导航结果：返回状态无效') + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/waypoint_follower.py b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/waypoint_follower.py new file mode 100644 index 0000000..07e31f9 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/fishbot_application/waypoint_follower.py @@ -0,0 +1,52 @@ +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + +def main(): + rclpy.init() + navigator = BasicNavigator() + navigator.waitUntilNav2Active() + # 创建点集 + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 0.0 + goal_pose1.pose.position.y = 0.0 + goal_pose1.pose.orientation.w = 1.0 + goal_poses.append(goal_pose1) + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 2.0 + goal_pose2.pose.position.y = 0.0 + goal_pose2.pose.orientation.w = 1.0 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = 2.0 + goal_pose3.pose.position.y = 2.0 + goal_pose3.pose.orientation.w = 1.0 + goal_poses.append(goal_pose3) + # 调用路点导航服务 + navigator.followWaypoints(goal_poses) + # 判断结束及获取反馈 + while not navigator.isTaskComplete(): + feedback = navigator.getFeedback() + navigator.get_logger().info( + f'当前目标编号：{feedback.current_waypoint}') + # 最终结果判断 + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + navigator.get_logger().info('导航结果：成功') + elif result == TaskResult.CANCELED: + navigator.get_logger().warn('导航结果：被取消') + elif result == TaskResult.FAILED: + navigator.get_logger().error('导航结果：失败') + else: + navigator.get_logger().error('导航结果：返回状态无效') + +if __name__ == '__main__': + main() diff --git a/chapt7/chapt7_ws/src/fishbot_application/package.xml b/chapt7/chapt7_ws/src/fishbot_application/package.xml new file mode 100644 index 0000000..8074680 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_application + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt7/chapt7_ws/src/fishbot_application/resource/fishbot_application b/chapt7/chapt7_ws/src/fishbot_application/resource/fishbot_application new file mode 100644 index 0000000..e69de29 diff --git a/chapt7/chapt7_ws/src/fishbot_application/setup.cfg b/chapt7/chapt7_ws/src/fishbot_application/setup.cfg new file mode 100644 index 0000000..b995d8e --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fishbot_application +[install] +install_scripts=$base/lib/fishbot_application diff --git a/chapt7/chapt7_ws/src/fishbot_application/setup.py b/chapt7/chapt7_ws/src/fishbot_application/setup.py new file mode 100644 index 0000000..98ebc51 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'fishbot_application' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'init_robot_pose=fishbot_application.init_robot_pose:main', + ], + }, +) diff --git a/chapt7/chapt7_ws/src/fishbot_application/test/test_copyright.py b/chapt7/chapt7_ws/src/fishbot_application/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt7/chapt7_ws/src/fishbot_application/test/test_flake8.py b/chapt7/chapt7_ws/src/fishbot_application/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt7/chapt7_ws/src/fishbot_application/test/test_pep257.py b/chapt7/chapt7_ws/src/fishbot_application/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt7/chapt7_ws/src/fishbot_application_cpp/CMakeLists.txt b/chapt7/chapt7_ws/src/fishbot_application_cpp/CMakeLists.txt new file mode 100644 index 0000000..f6075f8 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application_cpp/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_application_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rclcpp REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) + +add_executable(nav2pose src/nav2pose.cpp) +ament_target_dependencies(nav2pose rclcpp nav2_msgs rclcpp_action) + +install(TARGETS + nav2pose + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/chapt7/chapt7_ws/src/fishbot_application_cpp/package.xml b/chapt7/chapt7_ws/src/fishbot_application_cpp/package.xml new file mode 100644 index 0000000..efe88a5 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application_cpp/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_application_cpp + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt7/chapt7_ws/src/fishbot_application_cpp/src/nav2pose.cpp b/chapt7/chapt7_ws/src/fishbot_application_cpp/src/nav2pose.cpp new file mode 100644 index 0000000..a8e3383 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_application_cpp/src/nav2pose.cpp @@ -0,0 +1,71 @@ +#include + +#include "nav2_msgs/action/navigate_to_pose.hpp" // 导入导航动作消息的头文件 +#include "rclcpp/rclcpp.hpp" // 导入ROS 2的C++客户端库 +#include "rclcpp_action/rclcpp_action.hpp" // 导入ROS 2的C++ Action客户端库 + +using NavigationAction = nav2_msgs::action::NavigateToPose; // 定义导航动作类型为NavigateToPose + +class NavToPoseClient : public rclcpp::Node { + public: + using NavigationActionClient = rclcpp_action::Client; // 定义导航动作客户端类型 + using NavigationActionGoalHandle = + rclcpp_action::ClientGoalHandle; // 定义导航动作目标句柄类型 + + NavToPoseClient() : Node("nav_to_pose_client") { + // 创建导航动作客户端 + action_client_ = rclcpp_action::create_client( + this, "navigate_to_pose"); + } + + void sendGoal() { + // 等待导航动作服务器上线，等待时间为5秒 + while (!action_client_->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_INFO(get_logger(), "等待Action服务上线。"); + } + // 设置导航目标点 + auto goal_msg = NavigationAction::Goal(); + goal_msg.pose.header.frame_id = "map"; // 设置目标点的坐标系为地图坐标系 + goal_msg.pose.pose.position.x = 2.0f; // 设置目标点的x坐标为2.0 + goal_msg.pose.pose.position.y = 2.0f; // 设置目标点的y坐标为2.0 + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + // 设置请求目标结果回调函数 + send_goal_options.goal_response_callback = + [this](NavigationActionGoalHandle::SharedPtr goal_handle) { + if (goal_handle) { + RCLCPP_INFO(get_logger(), "目标点已被服务器接收"); + } + }; + // 设置移动过程反馈回调函数 + send_goal_options.feedback_callback = + [this]( + NavigationActionGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) { + (void)goal_handle; // 假装调用，避免 warning: unused + RCLCPP_INFO(this->get_logger(), "反馈剩余距离:%f", + feedback->distance_remaining); + }; + // 设置执行结果回调函数 + send_goal_options.result_callback = + [this](const NavigationActionGoalHandle::WrappedResult& result) { + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_INFO(this->get_logger(), "处理成功！"); + } + }; + // 发送导航目标点 + action_client_->async_send_goal(goal_msg, send_goal_options); + } + + NavigationActionClient::SharedPtr action_client_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->sendGoal(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/chapt7/chapt7_ws/src/fishbot_description/CMakeLists.txt b/chapt7/chapt7_ws/src/fishbot_description/CMakeLists.txt new file mode 100644 index 0000000..2a82fdf --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_description) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + + +install(DIRECTORY launch urdf config world + DESTINATION share/${PROJECT_NAME} +) + + +ament_package() diff --git a/chapt7/chapt7_ws/src/fishbot_description/LICENSE b/chapt7/chapt7_ws/src/fishbot_description/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/LICENSE @@ -0,0 +1,202 @@ + + 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. diff --git a/chapt7/chapt7_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml b/chapt7/chapt7_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml new file mode 100644 index 0000000..8a60aeb --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml @@ -0,0 +1,52 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + use_sim_time: true + fishbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + use_sim_time: true + fishbot_effort_controller: + type: effort_controllers/JointGroupEffortController + fishbot_diff_drive_controller: + type: diff_drive_controller/DiffDriveController + +fishbot_effort_controller: + ros__parameters: + joints: + - left_wheel_joint + - right_wheel_joint + command_interfaces: + - effort + state_interfaces: + - position + - velocity + - effort + + + +fishbot_diff_drive_controller: + ros__parameters: + left_wheel_names: ["left_wheel_joint"] + right_wheel_names: ["right_wheel_joint"] + + wheel_separation: 0.17 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.032 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_footprint + pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] + twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/fishbot_description/config/rviz/dispaly_model.rviz b/chapt7/chapt7_ws/src/fishbot_description/config/rviz/dispaly_model.rviz new file mode 100644 index 0000000..d624b06 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/config/rviz/dispaly_model.rviz @@ -0,0 +1,170 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 480 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.4101419746875763 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 777 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000026bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000026b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000026b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002d90000026b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1354 + X: 70 + Y: 27 diff --git a/chapt7/chapt7_ws/src/fishbot_description/launch/display_robot.launch.py b/chapt7/chapt7_ws/src/fishbot_description/launch/display_robot.launch.py new file mode 100644 index 0000000..3db487d --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/launch/display_robot.launch.py @@ -0,0 +1,42 @@ +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # 获取默认路径 + urdf_tutorial_path = get_package_share_directory('fishbot_description') + default_model_path = urdf_tutorial_path + '/urdf/first_robot.urdf' + default_rviz_config_path = urdf_tutorial_path + '/config/rviz/display_model.rviz' + # 为 Launch 声明参数 + action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument( + name='model', default_value=str(default_model_path), + description='URDF 的绝对路径') + # 获取文件内容生成新的参数 + robot_description = launch_ros.parameter_descriptions.ParameterValue( + launch.substitutions.Command( + ['xacro ', launch.substitutions.LaunchConfiguration('model')]), + value_type=str) + # 状态发布节点 + robot_state_publisher_node = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': robot_description}] + ) + # 关节状态发布节点 + joint_state_publisher_node = launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + ) + # RViz 节点 + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + arguments=['-d', default_rviz_config_path] + ) + return launch.LaunchDescription([ + action_declare_arg_mode_path, + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node + ]) diff --git a/chapt7/chapt7_ws/src/fishbot_description/launch/gazebo_sim.launch.py b/chapt7/chapt7_ws/src/fishbot_description/launch/gazebo_sim.launch.py new file mode 100644 index 0000000..16050ca --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/launch/gazebo_sim.launch.py @@ -0,0 +1,75 @@ +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + # 获取默认路径 + robot_name_in_model = "fishbot" + urdf_tutorial_path = get_package_share_directory('fishbot_description') + default_model_path = urdf_tutorial_path + '/urdf/fishbot/fishbot.urdf.xacro' + default_world_path = urdf_tutorial_path + '/world/custom_room.world' + # 为 Launch 声明参数 + action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument( + name='model', default_value=str(default_model_path), + description='URDF 的绝对路径') + # 获取文件内容生成新的参数 + robot_description = launch_ros.parameter_descriptions.ParameterValue( + launch.substitutions.Command( + ['xacro ', launch.substitutions.LaunchConfiguration('model')]), + value_type=str) + + robot_state_publisher_node = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': robot_description}] + ) + + # 通过 IncludeLaunchDescription 包含另外一个 launch 文件 + launch_gazebo = launch.actions.IncludeLaunchDescription( + PythonLaunchDescriptionSource([get_package_share_directory( + 'gazebo_ros'), '/launch', '/gazebo.launch.py']), + # 传递参数 + launch_arguments=[('world', default_world_path),('verbose','true')] + ) + # 请求 Gazebo 加载机器人 + spawn_entity_node = launch_ros.actions.Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-topic', '/robot_description', + '-entity', robot_name_in_model, ]) + + # 加载并激活 fishbot_joint_state_broadcaster 控制器 + load_joint_state_controller = launch.actions.ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'fishbot_joint_state_broadcaster'], + output='screen' + ) + + # 加载并激活 fishbot_effort_controller 控制器 + load_fishbot_effort_controller = launch.actions.ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_effort_controller'], + output='screen') + + load_fishbot_diff_drive_controller = launch.actions.ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_diff_drive_controller'], + output='screen') + + return launch.LaunchDescription([ + action_declare_arg_mode_path, + robot_state_publisher_node, + launch_gazebo, + spawn_entity_node, + # 事件动作，当加载机器人结束后执行 + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=spawn_entity_node, + on_exit=[load_joint_state_controller],) + ), + # 事件动作，load_fishbot_diff_drive_controller + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_fishbot_diff_drive_controller],) + ), + ]) diff --git a/chapt7/chapt7_ws/src/fishbot_description/package.xml b/chapt7/chapt7_ws/src/fishbot_description/package.xml new file mode 100644 index 0000000..575ade0 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_description + 0.0.0 + TODO: Package description + mzebra + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.gv b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.gv new file mode 100644 index 0000000..6d190c9 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.gv @@ -0,0 +1,8 @@ +digraph G { +node [shape=box]; +"base_link" [label="base_link"]; +"imu_link" [label="imu_link"]; +node [shape=ellipse, color=blue, fontcolor=blue]; +"base_link" -> "imu_joint" [label="xyz: 0 0 0.03 \nrpy: 0 -0 0"] +"imu_joint" -> "imu_link" +} diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.pdf b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.pdf new file mode 100644 index 0000000..a0a0955 Binary files /dev/null and b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.pdf differ diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.urdf b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.urdf new file mode 100644 index 0000000..5f706eb --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.urdf @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.urdf.xacro new file mode 100644 index 0000000..3a36fe7 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/first_robot.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/actuator/caster.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/actuator/caster.urdf.xacro new file mode 100644 index 0000000..e654233 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/actuator/caster.urdf.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/actuator/wheel.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/actuator/wheel.urdf.xacro new file mode 100644 index 0000000..6927798 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/actuator/wheel.urdf.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/base.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/base.urdf.xacro new file mode 100644 index 0000000..9f14ebc --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/base.urdf.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/common_inertia.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/common_inertia.xacro new file mode 100644 index 0000000..8486102 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/common_inertia.xacro @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro new file mode 100644 index 0000000..157b5ba --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro @@ -0,0 +1,45 @@ + + + + + + gazebo_ros2_control/GazeboSystem + + + + -1 + 1 + + + -0.1 + 0.1 + + + + + + + + -1 + 1 + + + -0.1 + 0.1 + + + + + + + + + $(find fishbot_description)/config/fishbot_ros2_controller.yaml + + /fishbot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel + /fishbot_diff_drive_controller/odom:=/odom + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro new file mode 100644 index 0000000..77c980b --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_control_plugin.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_control_plugin.xacro new file mode 100644 index 0000000..46d898a --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_control_plugin.xacro @@ -0,0 +1,31 @@ + + + + + + + / + cmd_vel:=cmd_vel + odom:=odom + + 30 + + left_wheel_joint + right_wheel_joint + + 0.2 + 0.064 + + 20 + 1.0 + + true + true + true + + odom + base_footprint + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro new file mode 100644 index 0000000..c69cbb4 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro @@ -0,0 +1,146 @@ + + + + + + + + / + ~/out:=scan + + sensor_msgs/LaserScan + laser_link + + true + true + 5 + 0 0 0 0 0 0 + + + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + + 0.120000 + 8.0 + 0.015000 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + / + ~/out:=imu + + false + + 100 + true + + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + + + + + + camera_optical_link + + true + 10 + + 1.5009831567 + + 800 + 600 + R8G8B8 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/camera.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/camera.urdf.xacro new file mode 100644 index 0000000..567a95f --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/camera.urdf.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/imu.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/imu.urdf.xacro new file mode 100644 index 0000000..2d0b45d --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/imu.urdf.xacro @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/laser.urdf.xacro b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/laser.urdf.xacro new file mode 100644 index 0000000..c9cabb3 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/urdf/fishbot/sensor/laser.urdf.xacro @@ -0,0 +1,73 @@ + + + + + + Gazebo/Black + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/world/custom_room.world b/chapt7/chapt7_ws/src/fishbot_description/world/custom_room.world new file mode 100644 index 0000000..a341114 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/world/custom_room.world @@ -0,0 +1,2745 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1435 337000000 + 1463 259419335 + 1684421669 225276651 + 1435337 + + 7.91236 4.68506 0 0 -0 0 + 1 1 1 + + 7.91236 4.68506 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -4.31975 4.20897 0 0 -0 0 + 1 1 1 + + -4.31975 4.20897 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -5.63889 4.25649 0 0 -0 0 + 1 1 1 + + -5.63889 4.25649 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.201253 -3.92041 0 0 -0 0 + 1 1 1 + + -0.201253 -3.92041 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.211824 -5.1227 0 0 -0 0 + 1 1 1 + + -0.211824 -5.1227 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.8359 -3.13197 0 0 -0 0 + 1 1 1 + + -8.8359 -3.13197 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -7.80202 -3.07286 0 0 -0 0 + 1 1 1 + + -7.80202 -3.07286 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -4.89593 2.8545 0 0 -0 0 + 1 1 1 + + -4.89593 2.8545 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 3.54156 1.36869 0 0 -0 0 + 1 1 1 + + 3.54156 1.36869 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 4.46016 1.36814 0 0 -0 0 + 1 1 1 + + 4.46016 1.36814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.972666 -0.903877 0 0 -0 0 + 1 1 1 + + -0.958745 4.60567 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.46625 -0.944332 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.958745 -6.49434 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.3837 -0.944332 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.09036 2.38658 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.39036 0.086579 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.580882 -4.77187 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.09411 -3.14872 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.36159 -2.71357 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -4.31159 -4.01357 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.958121 2.77839 0.5 0 -0 0 + 1 1 1 + + -0.958121 2.77839 0.5 0 -0 0 + 0 0 0 0 -0 0 + -0.004709 -9.78112 9.78158 0.712677 -0.009414 -4.3e-05 + -0.004709 -9.78112 9.78158 0 -0 0 + + + + -0.961048 0.717233 0.499993 -3e-06 -4e-06 -0 + 1 1 1 + + -0.961048 0.717233 0.499993 -3e-06 -4e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + -0.972666 -0.903877 0 0 -0 0 + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 5.50955 0 0 -0 0 + 0 + 0 + 0 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 9.43892 -0.040455 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 -5.59046 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -9.41108 -0.040455 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 4.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.11769 3.29046 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 6.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -4.41769 0.990456 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 3.47722 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.47722 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.391784 -3.86799 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 7.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 7.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.06678 -2.24484 0 0 -0 0 + 0 + 0 + 0 + + + + + + 6.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -6.38892 -1.80969 0 0 -0 0 + 0 + 0 + 0 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 2.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -3.33892 -3.10969 0 0 -0 -1.5708 + 0 + 0 + 0 + + 1 + + + 1 + + + 1 + + + 0 0.005 0.6 0 -0 0 + + + 0.9 0.01 1.2 + + + 10 + + + + + + + + + + + + + + + 0 0.005 0.6 0 -0 0 + + + 0.9 0.01 1.2 + + + + + + + + 0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + 10 + + + + + + + + + + + + + + + 0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + + + + + + -0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + 10 + + + + + + + + + + + + + + + -0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + + + + + + 0 -0.195 0.03 0 -0 0 + + + 0.88 0.4 0.06 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.03 0 -0 0 + + + 0.88 0.4 0.06 + + + + + + + + 0 -0.195 1.19 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 1.19 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + + 0 -0.195 0.43 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.43 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + + 0 -0.195 0.8 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.8 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + 0 + 0 + 0 + + 7.91236 4.68506 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -4.31975 4.20897 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -5.63889 4.25649 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -0.201253 -3.92041 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -0.211824 -5.1227 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -8.8359 -3.13197 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -7.80202 -3.07286 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + -4.89593 2.8545 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + 3.54156 1.36869 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + 4.46016 1.36814 0 0 -0 0 + + + -0.961046 0.717231 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + -0.958121 2.77839 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + + -0.170626 -11.744 24.2897 0 1.1458 1.5962 + orbit + perspective + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/world/room/model.config b/chapt7/chapt7_ws/src/fishbot_description/world/room/model.config new file mode 100644 index 0000000..6222143 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/world/room/model.config @@ -0,0 +1,11 @@ + + + room + 1.0 + model.sdf + + + + + + diff --git a/chapt7/chapt7_ws/src/fishbot_description/world/room/model.sdf b/chapt7/chapt7_ws/src/fishbot_description/world/room/model.sdf new file mode 100644 index 0000000..d499393 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_description/world/room/model.sdf @@ -0,0 +1,297 @@ + + + + -0.972666 -0.903877 0 0 -0 0 + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 5.50955 0 0 -0 0 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 9.43892 -0.040455 0 0 -0 -1.5708 + + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 -5.59046 0 0 -0 3.14159 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -9.41108 -0.040455 0 0 -0 1.5708 + + + + + + 4.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 4.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.11769 3.29046 0 0 -0 -1.5708 + + + + + + 6.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -4.41769 0.990456 0 0 -0 3.14159 + + + + + + 3.47722 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 3.47722 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.391784 -3.86799 0 0 -0 1.5708 + + + + + + 7.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 7.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.06678 -2.24484 0 0 -0 0 + + + + + + 6.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -6.38892 -1.80969 0 0 -0 0 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -3.33892 -3.10969 0 0 -0 -1.5708 + + 1 + + diff --git a/chapt7/chapt7_ws/src/fishbot_navigation2/CMakeLists.txt b/chapt7/chapt7_ws/src/fishbot_navigation2/CMakeLists.txt new file mode 100644 index 0000000..c185e3e --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_navigation2/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_navigation2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY launch maps config + DESTINATION share/${PROJECT_NAME} +) + + +ament_package() diff --git a/chapt7/chapt7_ws/src/fishbot_navigation2/config/nav2_params.yaml b/chapt7/chapt7_ws/src/fishbot_navigation2/config/nav2_params.yaml new file mode 100644 index 0000000..5f4b08f --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_navigation2/config/nav2_params.yaml @@ -0,0 +1,349 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/chapt7/chapt7_ws/src/fishbot_navigation2/launch/navigation2.launch.py b/chapt7/chapt7_ws/src/fishbot_navigation2/launch/navigation2.launch.py new file mode 100644 index 0000000..a55c3d5 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_navigation2/launch/navigation2.launch.py @@ -0,0 +1,49 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # 获取与拼接默认路径 + fishbot_navigation2_dir = get_package_share_directory( + 'fishbot_navigation2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + rviz_config_dir = os.path.join( + nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz') + + # 创建 Launch 配置 + use_sim_time = launch.substitutions.LaunchConfiguration( + 'use_sim_time', default='true') + map_yaml_path = launch.substitutions.LaunchConfiguration( + 'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml')) + nav2_param_path = launch.substitutions.LaunchConfiguration( + 'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml')) + + return launch.LaunchDescription([ + # 声明新的 Launch 参数 + launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time, + description='Use simulation (Gazebo) clock if true'), + launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path, + description='Full path to map file to load'), + launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path, + description='Full path to param file to load'), + + launch.actions.IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/bringup_launch.py']), + # 使用 Launch 参数替换原有参数 + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path}.items(), + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + parameters=[{'use_sim_time': use_sim_time}], + output='screen'), + ]) \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.pgm b/chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.pgm new file mode 100644 index 0000000..6bdfd22 Binary files /dev/null and b/chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.pgm differ diff --git a/chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.yaml b/chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.yaml new file mode 100644 index 0000000..d0cbed8 --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_navigation2/maps/room.yaml @@ -0,0 +1,7 @@ +image: room.pgm +mode: trinary +resolution: 0.05 +origin: [-10.4, -6.53, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/chapt7/chapt7_ws/src/fishbot_navigation2/package.xml b/chapt7/chapt7_ws/src/fishbot_navigation2/package.xml new file mode 100644 index 0000000..19065ec --- /dev/null +++ b/chapt7/chapt7_ws/src/fishbot_navigation2/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_navigation2 + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt8/chapt8_ws/src/autopatrol_interfaces/CMakeLists.txt b/chapt8/chapt8_ws/src/autopatrol_interfaces/CMakeLists.txt new file mode 100644 index 0000000..b2ed6b4 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_interfaces/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autopatrol_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rosidl_default_generators REQUIRED) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/SpeachText.srv" +) +ament_package() diff --git a/chapt8/chapt8_ws/src/autopatrol_interfaces/package.xml b/chapt8/chapt8_ws/src/autopatrol_interfaces/package.xml new file mode 100644 index 0000000..ccdf647 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_interfaces/package.xml @@ -0,0 +1,18 @@ + + + + autopatrol_interfaces + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_interface_packages + + ament_cmake + + diff --git a/chapt8/chapt8_ws/src/autopatrol_interfaces/srv/SpeachText.srv b/chapt8/chapt8_ws/src/autopatrol_interfaces/srv/SpeachText.srv new file mode 100644 index 0000000..440fa28 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_interfaces/srv/SpeachText.srv @@ -0,0 +1,3 @@ +string text # 合成文字 +--- +bool result # 合成结果 \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/__init__.py b/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/patrol_node.py b/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/patrol_node.py new file mode 100644 index 0000000..e1e3f1c --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/patrol_node.py @@ -0,0 +1,173 @@ +import rclpy +from geometry_msgs.msg import PoseStamped, Pose +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from tf2_ros import TransformListener, Buffer +from tf_transformations import euler_from_quaternion, quaternion_from_euler +from rclpy.duration import Duration +# 添加服务接口 +from autopatrol_interfaces.srv import SpeachText +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +import cv2 + +class PatrolNode(BasicNavigator): + def __init__(self, node_name='patrol_node'): + super().__init__(node_name) + # 导航相关定义 + self.declare_parameter('initial_point', [0.0, 0.0, 0.0]) + self.declare_parameter('target_points', [0.0, 0.0, 0.0, 1.0, 1.0, 1.57]) + self.initial_point_ = self.get_parameter('initial_point').value + self.target_points_ = self.get_parameter('target_points').value + # 实时位置获取 TF 相关定义 + self.buffer_ = Buffer() + self.listener_ = TransformListener(self.buffer_, self) + self.speach_client_ = self.create_client(SpeachText, 'speech_text') + + # 订阅与保存图像相关定义 + self.declare_parameter('image_save_path', '') + self.image_save_path = self.get_parameter('image_save_path').value + self.bridge = CvBridge() + self.latest_image = None + self.subscription_image = self.create_subscription( + Image, '/camera_sensor/image_raw', self.image_callback, 10) + + def image_callback(self, msg): + """ + 将最新的消息放到 latest_image 中 + """ + self.latest_image = msg + + def record_image(self): + """ + 记录图像 + """ + if self.latest_image is not None: + pose = self.get_current_pose() + cv_image = self.bridge.imgmsg_to_cv2(self.latest_image) + cv2.imwrite(f'{self.image_save_path}image_{pose.translation.x:3.2f}_{pose.translation.y:3.2f}.png', cv_image) + + + def speach_text(self, text): + """ + 调用服务播放语音 + """ + while not self.speach_client_.wait_for_service(timeout_sec=1.0): + self.get_logger().info('语合成服务未上线，等待中。。。') + + request = SpeachText.Request() + request.text = text + future = self.speach_client_.call_async(request) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + result = future.result().result + if result: + self.get_logger().info(f'语音合成成功：{text}') + else: + self.get_logger().warn(f'语音合成失败：{text}') + else: + self.get_logger().warn('语音合成服务请求失败') + + def get_pose_by_xyyaw(self, x, y, yaw): + """ + 通过 x,y,yaw 合成 PoseStamped + """ + pose = PoseStamped() + pose.header.frame_id = 'map' + pose.pose.position.x = x + pose.pose.position.y = y + rotation_quat = quaternion_from_euler(0, 0, yaw) + pose.pose.orientation.x = rotation_quat[0] + pose.pose.orientation.y = rotation_quat[1] + pose.pose.orientation.z = rotation_quat[2] + pose.pose.orientation.w = rotation_quat[3] + return pose + + def init_robot_pose(self): + """ + 初始化机器人位姿 + """ + # 从参数获取初始化点 + self.initial_point_ = self.get_parameter('initial_point').value + # 合成位姿并进行初始化 + self.setInitialPose(self.get_pose_by_xyyaw( + self.initial_point_[0], self.initial_point_[1], self.initial_point_[2])) + # 等待直到导航激活 + self.waitUntilNav2Active() + + def get_target_points(self): + """ + 通过参数值获取目标点集合 + """ + points = [] + self.target_points_ = self.get_parameter('target_points').value + for index in range(int(len(self.target_points_)/3)): + x = self.target_points_[index*3] + y = self.target_points_[index*3+1] + yaw = self.target_points_[index*3+2] + points.append([x, y, yaw]) + self.get_logger().info(f'获取到目标点: {index}->({x},{y},{yaw})') + return points + + def nav_to_pose(self, target_pose): + """ + 导航到指定位姿 + """ + self.waitUntilNav2Active() + result = self.goToPose(target_pose) + while not self.isTaskComplete(): + feedback = self.getFeedback() + if feedback: + self.get_logger().info(f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达') + # 最终结果判断 + result = self.getResult() + if result == TaskResult.SUCCEEDED: + self.get_logger().info('导航结果：成功') + elif result == TaskResult.CANCELED: + self.get_logger().warn('导航结果：被取消') + elif result == TaskResult.FAILED: + self.get_logger().error('导航结果：失败') + else: + self.get_logger().error('导航结果：返回状态无效') + + def get_current_pose(self): + """ + 通过TF获取当前位姿 + """ + while rclpy.ok(): + try: + tf = self.buffer_.lookup_transform( + 'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1)) + transform = tf.transform + rotation_euler = euler_from_quaternion([ + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w + ]) + self.get_logger().info( + f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}') + return transform + except Exception as e: + self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}') + +def main(): + rclpy.init() + patrol = PatrolNode() + patrol.speach_text(text='正在初始化位置') + patrol.init_robot_pose() + patrol.speach_text(text='位置初始化完成') + + while rclpy.ok(): + for point in patrol.get_target_points(): + x, y, yaw = point[0], point[1], point[2] + # 导航到目标点 + target_pose = patrol.get_pose_by_xyyaw(x, y, yaw) + patrol.speach_text(text=f'准备前往目标点{x},{y}') + patrol.nav_to_pose(target_pose) + patrol.speach_text(text=f"已到达目标点{x},{y},准备记录图像") + patrol.record_image() + patrol.speach_text(text=f"图像记录完成") + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/speaker.py b/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/speaker.py new file mode 100644 index 0000000..32e7269 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/autopatrol_robot/speaker.py @@ -0,0 +1,26 @@ +import rclpy +from rclpy.node import Node +from autopatrol_interfaces.srv import SpeachText +import espeakng + +class Speaker(Node): + def __init__(self, node_name): + super().__init__(node_name) + self.speech_service = self.create_service( + SpeachText, 'speech_text', self.speak_text_callback) + self.speaker = espeakng.Speaker() + self.speaker.voice = 'zh' + + def speak_text_callback(self, request, response): + self.get_logger().info('正在朗读 %s' % request.text) + self.speaker.say(request.text) + self.speaker.wait() + response.result = True + return response + + +def main(args=None): + rclpy.init(args=args) + node = Speaker('speaker') + rclpy.spin(node) + rclpy.shutdown() \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/config/patrol_config.yaml b/chapt8/chapt8_ws/src/autopatrol_robot/config/patrol_config.yaml new file mode 100644 index 0000000..cfef5b3 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/config/patrol_config.yaml @@ -0,0 +1,10 @@ +patrol_node: + ros__parameters: + initial_point: [0.0, 0.0, 0.0] + target_points: [ + 0.0,0.0,0.0, + 1.0,2.0,3.14, + -4.5,1.5,1.57, + -8.0,-5.0,1.57, + 1.0,-5.0,3.14, + ] \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/launch/autopatrol.launch.py b/chapt8/chapt8_ws/src/autopatrol_robot/launch/autopatrol.launch.py new file mode 100644 index 0000000..819362b --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/launch/autopatrol.launch.py @@ -0,0 +1,28 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # 获取与拼接默认路径 + autopatrol_robot_dir = get_package_share_directory( + 'autopatrol_robot') + patrol_config_path = os.path.join( + autopatrol_robot_dir, 'config', 'patrol_config.yaml') + + action_node_turtle_control = launch_ros.actions.Node( + package='autopatrol_robot', + executable='patrol_node', + parameters=[patrol_config_path] + ) + action_node_patrol_client = launch_ros.actions.Node( + package='autopatrol_robot', + executable='speaker', + ) + + return launch.LaunchDescription([ + action_node_turtle_control, + action_node_patrol_client, + ]) \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/package.xml b/chapt8/chapt8_ws/src/autopatrol_robot/package.xml new file mode 100644 index 0000000..aa36510 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/package.xml @@ -0,0 +1,18 @@ + + + + autopatrol_robot + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/resource/autopatrol_robot b/chapt8/chapt8_ws/src/autopatrol_robot/resource/autopatrol_robot new file mode 100644 index 0000000..e69de29 diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/setup.cfg b/chapt8/chapt8_ws/src/autopatrol_robot/setup.cfg new file mode 100644 index 0000000..60e9700 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/autopatrol_robot +[install] +install_scripts=$base/lib/autopatrol_robot diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/setup.py b/chapt8/chapt8_ws/src/autopatrol_robot/setup.py new file mode 100644 index 0000000..8b889e3 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup +from glob import glob +package_name = 'autopatrol_robot' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name+'/config', ['config/patrol_config.yaml']), + ('share/' + package_name+'/launch', glob('launch/*.launch.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'patrol_node=autopatrol_robot.patrol_node:main', + 'speaker=autopatrol_robot.speaker:main', + ], + }, +) diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/test/test_copyright.py b/chapt8/chapt8_ws/src/autopatrol_robot/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/test/test_flake8.py b/chapt8/chapt8_ws/src/autopatrol_robot/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt8/chapt8_ws/src/autopatrol_robot/test/test_pep257.py b/chapt8/chapt8_ws/src/autopatrol_robot/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt8/chapt8_ws/src/autopatrol_robot/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/__init__.py b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/get_robot_pose.py b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/get_robot_pose.py new file mode 100644 index 0000000..fddcd8e --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/get_robot_pose.py @@ -0,0 +1,40 @@ +import rclpy +from rclpy.node import Node +from tf2_ros import TransformListener, Buffer +from tf_transformations import euler_from_quaternion + + +class TFListener(Node): + + def __init__(self): + super().__init__('tf2_listener') + self.buffer = Buffer() + self.listener = TransformListener(self.buffer, self) + self.timer = self.create_timer(1, self.get_transform) + + def get_transform(self): + try: + tf = self.buffer.lookup_transform( + 'map', 'base_footprint', rclpy.time.Time(seconds=0), rclpy.time.Duration(seconds=1)) + transform = tf.transform + rotation_euler = euler_from_quaternion([ + transform.rotation.x, + transform.rotation.y, + transform.rotation.z, + transform.rotation.w + ]) + self.get_logger().info( + f'平移:{transform.translation},旋转四元数:{transform.rotation}:旋转欧拉角:{rotation_euler}') + except Exception as e: + self.get_logger().warn(f'不能够获取坐标变换，原因: {str(e)}') + + +def main(): + rclpy.init() + node = TFListener() + rclpy.spin(node) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/init_robot_pose.py b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/init_robot_pose.py new file mode 100644 index 0000000..8ed9029 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/init_robot_pose.py @@ -0,0 +1,21 @@ +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator +import rclpy + + +def main(): + rclpy.init() + navigator = BasicNavigator() + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.w = 1.0 + navigator.setInitialPose(initial_pose) + navigator.waitUntilNav2Active() + rclpy.spin(navigator) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/nav_to_pose.py b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/nav_to_pose.py new file mode 100644 index 0000000..45ff081 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/nav_to_pose.py @@ -0,0 +1,40 @@ +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + + +def main(): + rclpy.init() + navigator = BasicNavigator() + # 等待导航启动完成 + navigator.waitUntilNav2Active() + # 设置目标点坐标 + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + goal_pose.pose.position.x = 1.0 + goal_pose.pose.position.y = 1.0 + goal_pose.pose.orientation.w = 1.0 + # 发送目标接收反馈结果 + navigator.goToPose(goal_pose) + while not navigator.isTaskComplete(): + feedback = navigator.getFeedback() + navigator.get_logger().info( + f'预计: {Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9} s 后到达') + # 超时自动取消 + if Duration.from_msg(feedback.navigation_time) > Duration(seconds=600.0): + navigator.cancelTask() + # 最终结果判断 + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + navigator.get_logger().info('导航结果：成功') + elif result == TaskResult.CANCELED: + navigator.get_logger().warn('导航结果：被取消') + elif result == TaskResult.FAILED: + navigator.get_logger().error('导航结果：失败') + else: + navigator.get_logger().error('导航结果：返回状态无效') + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/waypoint_follower.py b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/waypoint_follower.py new file mode 100644 index 0000000..07e31f9 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/fishbot_application/waypoint_follower.py @@ -0,0 +1,52 @@ +from geometry_msgs.msg import PoseStamped +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +import rclpy +from rclpy.duration import Duration + +def main(): + rclpy.init() + navigator = BasicNavigator() + navigator.waitUntilNav2Active() + # 创建点集 + goal_poses = [] + goal_pose1 = PoseStamped() + goal_pose1.header.frame_id = 'map' + goal_pose1.header.stamp = navigator.get_clock().now().to_msg() + goal_pose1.pose.position.x = 0.0 + goal_pose1.pose.position.y = 0.0 + goal_pose1.pose.orientation.w = 1.0 + goal_poses.append(goal_pose1) + goal_pose2 = PoseStamped() + goal_pose2.header.frame_id = 'map' + goal_pose2.header.stamp = navigator.get_clock().now().to_msg() + goal_pose2.pose.position.x = 2.0 + goal_pose2.pose.position.y = 0.0 + goal_pose2.pose.orientation.w = 1.0 + goal_poses.append(goal_pose2) + goal_pose3 = PoseStamped() + goal_pose3.header.frame_id = 'map' + goal_pose3.header.stamp = navigator.get_clock().now().to_msg() + goal_pose3.pose.position.x = 2.0 + goal_pose3.pose.position.y = 2.0 + goal_pose3.pose.orientation.w = 1.0 + goal_poses.append(goal_pose3) + # 调用路点导航服务 + navigator.followWaypoints(goal_poses) + # 判断结束及获取反馈 + while not navigator.isTaskComplete(): + feedback = navigator.getFeedback() + navigator.get_logger().info( + f'当前目标编号：{feedback.current_waypoint}') + # 最终结果判断 + result = navigator.getResult() + if result == TaskResult.SUCCEEDED: + navigator.get_logger().info('导航结果：成功') + elif result == TaskResult.CANCELED: + navigator.get_logger().warn('导航结果：被取消') + elif result == TaskResult.FAILED: + navigator.get_logger().error('导航结果：失败') + else: + navigator.get_logger().error('导航结果：返回状态无效') + +if __name__ == '__main__': + main() diff --git a/chapt8/chapt8_ws/src/fishbot_application/package.xml b/chapt8/chapt8_ws/src/fishbot_application/package.xml new file mode 100644 index 0000000..8074680 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_application + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chapt8/chapt8_ws/src/fishbot_application/resource/fishbot_application b/chapt8/chapt8_ws/src/fishbot_application/resource/fishbot_application new file mode 100644 index 0000000..e69de29 diff --git a/chapt8/chapt8_ws/src/fishbot_application/setup.cfg b/chapt8/chapt8_ws/src/fishbot_application/setup.cfg new file mode 100644 index 0000000..b995d8e --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/fishbot_application +[install] +install_scripts=$base/lib/fishbot_application diff --git a/chapt8/chapt8_ws/src/fishbot_application/setup.py b/chapt8/chapt8_ws/src/fishbot_application/setup.py new file mode 100644 index 0000000..98ebc51 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'fishbot_application' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='fishros', + maintainer_email='87068644+fishros@users.noreply.github.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'init_robot_pose=fishbot_application.init_robot_pose:main', + ], + }, +) diff --git a/chapt8/chapt8_ws/src/fishbot_application/test/test_copyright.py b/chapt8/chapt8_ws/src/fishbot_application/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/chapt8/chapt8_ws/src/fishbot_application/test/test_flake8.py b/chapt8/chapt8_ws/src/fishbot_application/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/chapt8/chapt8_ws/src/fishbot_application/test/test_pep257.py b/chapt8/chapt8_ws/src/fishbot_application/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/chapt8/chapt8_ws/src/fishbot_application_cpp/CMakeLists.txt b/chapt8/chapt8_ws/src/fishbot_application_cpp/CMakeLists.txt new file mode 100644 index 0000000..f6075f8 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application_cpp/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_application_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rclcpp REQUIRED) +find_package(nav2_msgs REQUIRED) +find_package(rclcpp_action REQUIRED) + +add_executable(nav2pose src/nav2pose.cpp) +ament_target_dependencies(nav2pose rclcpp nav2_msgs rclcpp_action) + +install(TARGETS + nav2pose + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/chapt8/chapt8_ws/src/fishbot_application_cpp/package.xml b/chapt8/chapt8_ws/src/fishbot_application_cpp/package.xml new file mode 100644 index 0000000..efe88a5 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application_cpp/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_application_cpp + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt8/chapt8_ws/src/fishbot_application_cpp/src/nav2pose.cpp b/chapt8/chapt8_ws/src/fishbot_application_cpp/src/nav2pose.cpp new file mode 100644 index 0000000..a8e3383 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_application_cpp/src/nav2pose.cpp @@ -0,0 +1,71 @@ +#include + +#include "nav2_msgs/action/navigate_to_pose.hpp" // 导入导航动作消息的头文件 +#include "rclcpp/rclcpp.hpp" // 导入ROS 2的C++客户端库 +#include "rclcpp_action/rclcpp_action.hpp" // 导入ROS 2的C++ Action客户端库 + +using NavigationAction = nav2_msgs::action::NavigateToPose; // 定义导航动作类型为NavigateToPose + +class NavToPoseClient : public rclcpp::Node { + public: + using NavigationActionClient = rclcpp_action::Client; // 定义导航动作客户端类型 + using NavigationActionGoalHandle = + rclcpp_action::ClientGoalHandle; // 定义导航动作目标句柄类型 + + NavToPoseClient() : Node("nav_to_pose_client") { + // 创建导航动作客户端 + action_client_ = rclcpp_action::create_client( + this, "navigate_to_pose"); + } + + void sendGoal() { + // 等待导航动作服务器上线，等待时间为5秒 + while (!action_client_->wait_for_action_server(std::chrono::seconds(5))) { + RCLCPP_INFO(get_logger(), "等待Action服务上线。"); + } + // 设置导航目标点 + auto goal_msg = NavigationAction::Goal(); + goal_msg.pose.header.frame_id = "map"; // 设置目标点的坐标系为地图坐标系 + goal_msg.pose.pose.position.x = 2.0f; // 设置目标点的x坐标为2.0 + goal_msg.pose.pose.position.y = 2.0f; // 设置目标点的y坐标为2.0 + + auto send_goal_options = + rclcpp_action::Client::SendGoalOptions(); + // 设置请求目标结果回调函数 + send_goal_options.goal_response_callback = + [this](NavigationActionGoalHandle::SharedPtr goal_handle) { + if (goal_handle) { + RCLCPP_INFO(get_logger(), "目标点已被服务器接收"); + } + }; + // 设置移动过程反馈回调函数 + send_goal_options.feedback_callback = + [this]( + NavigationActionGoalHandle::SharedPtr goal_handle, + const std::shared_ptr feedback) { + (void)goal_handle; // 假装调用，避免 warning: unused + RCLCPP_INFO(this->get_logger(), "反馈剩余距离:%f", + feedback->distance_remaining); + }; + // 设置执行结果回调函数 + send_goal_options.result_callback = + [this](const NavigationActionGoalHandle::WrappedResult& result) { + if (result.code == rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_INFO(this->get_logger(), "处理成功！"); + } + }; + // 发送导航目标点 + action_client_->async_send_goal(goal_msg, send_goal_options); + } + + NavigationActionClient::SharedPtr action_client_; +}; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->sendGoal(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/chapt8/chapt8_ws/src/fishbot_description/CMakeLists.txt b/chapt8/chapt8_ws/src/fishbot_description/CMakeLists.txt new file mode 100644 index 0000000..2a82fdf --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_description) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + + +install(DIRECTORY launch urdf config world + DESTINATION share/${PROJECT_NAME} +) + + +ament_package() diff --git a/chapt8/chapt8_ws/src/fishbot_description/LICENSE b/chapt8/chapt8_ws/src/fishbot_description/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/LICENSE @@ -0,0 +1,202 @@ + + 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. diff --git a/chapt8/chapt8_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml b/chapt8/chapt8_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml new file mode 100644 index 0000000..8a60aeb --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/config/fishbot_ros2_controller.yaml @@ -0,0 +1,52 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + use_sim_time: true + fishbot_joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + use_sim_time: true + fishbot_effort_controller: + type: effort_controllers/JointGroupEffortController + fishbot_diff_drive_controller: + type: diff_drive_controller/DiffDriveController + +fishbot_effort_controller: + ros__parameters: + joints: + - left_wheel_joint + - right_wheel_joint + command_interfaces: + - effort + state_interfaces: + - position + - velocity + - effort + + + +fishbot_diff_drive_controller: + ros__parameters: + left_wheel_names: ["left_wheel_joint"] + right_wheel_names: ["right_wheel_joint"] + + wheel_separation: 0.17 + #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal + wheel_radius: 0.032 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: base_footprint + pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] + twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] + + open_loop: true + enable_odom_tf: true + + cmd_vel_timeout: 0.5 + #publish_limited_velocity: true + use_stamped_vel: false + #velocity_rolling_window_size: 10 \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/fishbot_description/config/rviz/dispaly_model.rviz b/chapt8/chapt8_ws/src/fishbot_description/config/rviz/dispaly_model.rviz new file mode 100644 index 0000000..d624b06 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/config/rviz/dispaly_model.rviz @@ -0,0 +1,170 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 480 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.4101419746875763 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 777 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000026bfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000026b000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000026bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000026b000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000054a0000003efc0100000002fb0000000800540069006d006501000000000000054a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002d90000026b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1354 + X: 70 + Y: 27 diff --git a/chapt8/chapt8_ws/src/fishbot_description/launch/display_robot.launch.py b/chapt8/chapt8_ws/src/fishbot_description/launch/display_robot.launch.py new file mode 100644 index 0000000..3db487d --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/launch/display_robot.launch.py @@ -0,0 +1,42 @@ +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # 获取默认路径 + urdf_tutorial_path = get_package_share_directory('fishbot_description') + default_model_path = urdf_tutorial_path + '/urdf/first_robot.urdf' + default_rviz_config_path = urdf_tutorial_path + '/config/rviz/display_model.rviz' + # 为 Launch 声明参数 + action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument( + name='model', default_value=str(default_model_path), + description='URDF 的绝对路径') + # 获取文件内容生成新的参数 + robot_description = launch_ros.parameter_descriptions.ParameterValue( + launch.substitutions.Command( + ['xacro ', launch.substitutions.LaunchConfiguration('model')]), + value_type=str) + # 状态发布节点 + robot_state_publisher_node = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': robot_description}] + ) + # 关节状态发布节点 + joint_state_publisher_node = launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + ) + # RViz 节点 + rviz_node = launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + arguments=['-d', default_rviz_config_path] + ) + return launch.LaunchDescription([ + action_declare_arg_mode_path, + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node + ]) diff --git a/chapt8/chapt8_ws/src/fishbot_description/launch/gazebo_sim.launch.py b/chapt8/chapt8_ws/src/fishbot_description/launch/gazebo_sim.launch.py new file mode 100644 index 0000000..16050ca --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/launch/gazebo_sim.launch.py @@ -0,0 +1,75 @@ +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + # 获取默认路径 + robot_name_in_model = "fishbot" + urdf_tutorial_path = get_package_share_directory('fishbot_description') + default_model_path = urdf_tutorial_path + '/urdf/fishbot/fishbot.urdf.xacro' + default_world_path = urdf_tutorial_path + '/world/custom_room.world' + # 为 Launch 声明参数 + action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument( + name='model', default_value=str(default_model_path), + description='URDF 的绝对路径') + # 获取文件内容生成新的参数 + robot_description = launch_ros.parameter_descriptions.ParameterValue( + launch.substitutions.Command( + ['xacro ', launch.substitutions.LaunchConfiguration('model')]), + value_type=str) + + robot_state_publisher_node = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': robot_description}] + ) + + # 通过 IncludeLaunchDescription 包含另外一个 launch 文件 + launch_gazebo = launch.actions.IncludeLaunchDescription( + PythonLaunchDescriptionSource([get_package_share_directory( + 'gazebo_ros'), '/launch', '/gazebo.launch.py']), + # 传递参数 + launch_arguments=[('world', default_world_path),('verbose','true')] + ) + # 请求 Gazebo 加载机器人 + spawn_entity_node = launch_ros.actions.Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=['-topic', '/robot_description', + '-entity', robot_name_in_model, ]) + + # 加载并激活 fishbot_joint_state_broadcaster 控制器 + load_joint_state_controller = launch.actions.ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'fishbot_joint_state_broadcaster'], + output='screen' + ) + + # 加载并激活 fishbot_effort_controller 控制器 + load_fishbot_effort_controller = launch.actions.ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_effort_controller'], + output='screen') + + load_fishbot_diff_drive_controller = launch.actions.ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active','fishbot_diff_drive_controller'], + output='screen') + + return launch.LaunchDescription([ + action_declare_arg_mode_path, + robot_state_publisher_node, + launch_gazebo, + spawn_entity_node, + # 事件动作，当加载机器人结束后执行 + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=spawn_entity_node, + on_exit=[load_joint_state_controller],) + ), + # 事件动作，load_fishbot_diff_drive_controller + launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_fishbot_diff_drive_controller],) + ), + ]) diff --git a/chapt8/chapt8_ws/src/fishbot_description/package.xml b/chapt8/chapt8_ws/src/fishbot_description/package.xml new file mode 100644 index 0000000..575ade0 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_description + 0.0.0 + TODO: Package description + mzebra + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.gv b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.gv new file mode 100644 index 0000000..6d190c9 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.gv @@ -0,0 +1,8 @@ +digraph G { +node [shape=box]; +"base_link" [label="base_link"]; +"imu_link" [label="imu_link"]; +node [shape=ellipse, color=blue, fontcolor=blue]; +"base_link" -> "imu_joint" [label="xyz: 0 0 0.03 \nrpy: 0 -0 0"] +"imu_joint" -> "imu_link" +} diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.pdf b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.pdf new file mode 100644 index 0000000..a0a0955 Binary files /dev/null and b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.pdf differ diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.urdf b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.urdf new file mode 100644 index 0000000..5f706eb --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.urdf @@ -0,0 +1,44 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.urdf.xacro new file mode 100644 index 0000000..3a36fe7 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/first_robot.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/actuator/caster.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/actuator/caster.urdf.xacro new file mode 100644 index 0000000..e654233 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/actuator/caster.urdf.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/actuator/wheel.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/actuator/wheel.urdf.xacro new file mode 100644 index 0000000..6927798 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/actuator/wheel.urdf.xacro @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/base.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/base.urdf.xacro new file mode 100644 index 0000000..9f14ebc --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/base.urdf.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/common_inertia.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/common_inertia.xacro new file mode 100644 index 0000000..8486102 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/common_inertia.xacro @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro new file mode 100644 index 0000000..157b5ba --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/fishbot.ros2_control.xacro @@ -0,0 +1,45 @@ + + + + + + gazebo_ros2_control/GazeboSystem + + + + -1 + 1 + + + -0.1 + 0.1 + + + + + + + + -1 + 1 + + + -0.1 + 0.1 + + + + + + + + + $(find fishbot_description)/config/fishbot_ros2_controller.yaml + + /fishbot_diff_drive_controller/cmd_vel_unstamped:=/cmd_vel + /fishbot_diff_drive_controller/odom:=/odom + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro new file mode 100644 index 0000000..77c980b --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/fishbot.urdf.xacro @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_control_plugin.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_control_plugin.xacro new file mode 100644 index 0000000..46d898a --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_control_plugin.xacro @@ -0,0 +1,31 @@ + + + + + + + / + cmd_vel:=cmd_vel + odom:=odom + + 30 + + left_wheel_joint + right_wheel_joint + + 0.2 + 0.064 + + 20 + 1.0 + + true + true + true + + odom + base_footprint + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro new file mode 100644 index 0000000..c69cbb4 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/plugins/gazebo_sensor_plugin.xacro @@ -0,0 +1,146 @@ + + + + + + + + / + ~/out:=scan + + sensor_msgs/LaserScan + laser_link + + true + true + 5 + 0 0 0 0 0 0 + + + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + + 0.120000 + 8.0 + 0.015000 + + + + gaussian + 0.0 + 0.01 + + + + + + + + + + + + / + ~/out:=imu + + false + + 100 + true + + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + + + + + + camera_optical_link + + true + 10 + + 1.5009831567 + + 800 + 600 + R8G8B8 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/camera.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/camera.urdf.xacro new file mode 100644 index 0000000..567a95f --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/camera.urdf.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/imu.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/imu.urdf.xacro new file mode 100644 index 0000000..2d0b45d --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/imu.urdf.xacro @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/laser.urdf.xacro b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/laser.urdf.xacro new file mode 100644 index 0000000..c9cabb3 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/urdf/fishbot/sensor/laser.urdf.xacro @@ -0,0 +1,73 @@ + + + + + + Gazebo/Black + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/world/custom_room.world b/chapt8/chapt8_ws/src/fishbot_description/world/custom_room.world new file mode 100644 index 0000000..a341114 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/world/custom_room.world @@ -0,0 +1,2745 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + 0 + 0 + 0 + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + 0 + 0 + 0 + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 1435 337000000 + 1463 259419335 + 1684421669 225276651 + 1435337 + + 7.91236 4.68506 0 0 -0 0 + 1 1 1 + + 7.91236 4.68506 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -4.31975 4.20897 0 0 -0 0 + 1 1 1 + + -4.31975 4.20897 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -5.63889 4.25649 0 0 -0 0 + 1 1 1 + + -5.63889 4.25649 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.201253 -3.92041 0 0 -0 0 + 1 1 1 + + -0.201253 -3.92041 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.211824 -5.1227 0 0 -0 0 + 1 1 1 + + -0.211824 -5.1227 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -8.8359 -3.13197 0 0 -0 0 + 1 1 1 + + -8.8359 -3.13197 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -7.80202 -3.07286 0 0 -0 0 + 1 1 1 + + -7.80202 -3.07286 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -4.89593 2.8545 0 0 -0 0 + 1 1 1 + + -4.89593 2.8545 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 3.54156 1.36869 0 0 -0 0 + 1 1 1 + + 3.54156 1.36869 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 4.46016 1.36814 0 0 -0 0 + 1 1 1 + + 4.46016 1.36814 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.972666 -0.903877 0 0 -0 0 + 1 1 1 + + -0.958745 4.60567 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 8.46625 -0.944332 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.958745 -6.49434 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -10.3837 -0.944332 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -2.09036 2.38658 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -5.39036 0.086579 0 0 -0 3.14159 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -0.580882 -4.77187 0 0 -0 1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 3.09411 -3.14872 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -7.36159 -2.71357 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + -4.31159 -4.01357 0 0 0 -1.5708 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + -0.958121 2.77839 0.5 0 -0 0 + 1 1 1 + + -0.958121 2.77839 0.5 0 -0 0 + 0 0 0 0 -0 0 + -0.004709 -9.78112 9.78158 0.712677 -0.009414 -4.3e-05 + -0.004709 -9.78112 9.78158 0 -0 0 + + + + -0.961048 0.717233 0.499993 -3e-06 -4e-06 -0 + 1 1 1 + + -0.961048 0.717233 0.499993 -3e-06 -4e-06 -0 + 0 0 0 0 -0 0 + 0 0 -9.8 0 -0 0 + 0 0 -9.8 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + -0.972666 -0.903877 0 0 -0 0 + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 5.50955 0 0 -0 0 + 0 + 0 + 0 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 9.43892 -0.040455 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 -5.59046 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -9.41108 -0.040455 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 4.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 4.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.11769 3.29046 0 0 -0 -1.5708 + 0 + 0 + 0 + + + + + + 6.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -4.41769 0.990456 0 0 -0 3.14159 + 0 + 0 + 0 + + + + + + 3.47722 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 3.47722 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.391784 -3.86799 0 0 -0 1.5708 + 0 + 0 + 0 + + + + + + 7.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 7.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.06678 -2.24484 0 0 -0 0 + 0 + 0 + 0 + + + + + + 6.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 6.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -6.38892 -1.80969 0 0 -0 0 + 0 + 0 + 0 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + 10 + + + + + + + + + + + + + + + 0 0 1.25 0 -0 0 + + + 2.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -3.33892 -3.10969 0 0 -0 -1.5708 + 0 + 0 + 0 + + 1 + + + 1 + + + 1 + + + 0 0.005 0.6 0 -0 0 + + + 0.9 0.01 1.2 + + + 10 + + + + + + + + + + + + + + + 0 0.005 0.6 0 -0 0 + + + 0.9 0.01 1.2 + + + + + + + + 0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + 10 + + + + + + + + + + + + + + + 0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + + + + + + -0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + 10 + + + + + + + + + + + + + + + -0.45 -0.195 0.6 0 -0 0 + + + 0.02 0.4 1.2 + + + + + + + + 0 -0.195 0.03 0 -0 0 + + + 0.88 0.4 0.06 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.03 0 -0 0 + + + 0.88 0.4 0.06 + + + + + + + + 0 -0.195 1.19 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 1.19 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + + 0 -0.195 0.43 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.43 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + + 0 -0.195 0.8 0 -0 0 + + + 0.88 0.4 0.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.195 0.8 0 -0 0 + + + 0.88 0.4 0.02 + + + + + + + 0 + 0 + 0 + + 7.91236 4.68506 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -4.31975 4.20897 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -5.63889 4.25649 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -0.201253 -3.92041 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -0.211824 -5.1227 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -8.8359 -3.13197 0 0 -0 0 + + + 1 + + + 0 0 -1 0 -0 0 + + 2.05 + 0 + 0 + 2.05 + 0 + 2.05 + + 25 + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + 10 + + + + + + + + + + + + + + + 0.235 0 0.51 0 -0 0 + + + 0.02 0.45 1.02 + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 0.51 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + 10 + + + + + + + + + + + + + + + 0 -0.235 0.51 0 -0 0 + + + 0.45 0.02 1.02 + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + 10 + + + + + + + + + + + + + + + 0 0 1.01 0 -0 0 + + + 0.45 0.45 0.02 + + + + + + + 0 + 0 + 0 + + -7.80202 -3.07286 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + -4.89593 2.8545 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + 3.54156 1.36869 0 0 -0 0 + + + 1 + + + 0 0 0.755 0 -0 0 + + + 0.913 0.913 0.04 + + + 10 + + + + + + + + + + + + + + + 0 0 0.37 0 -0 0 + + + 0.042 0.042 0.74 + + + 10 + + + + + + + + + + + + + + + 0 0 0.02 0 -0 0 + + + 0.56 0.56 0.04 + + + 10 + + + + + + + + + + + + + + + + + model://cafe_table/meshes/cafe_table.dae + + + + 0 + 0 + 0 + + 4.46016 1.36814 0 0 -0 0 + + + -0.961046 0.717231 0.5 0 -0 0 + + + 1 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + 0 0 0 0 -0 0 + + + + + 0.5 + 1 + + + 10 + + + + + + + + + + + + + + + + + 0.5 + 1 + + + + + + + 0 + 0 + 0 + + + + -0.958121 2.77839 0.5 0 -0 0 + + + 1 + + 0.166667 + 0 + 0 + 0.166667 + 0 + 0.166667 + + 0 0 0 0 -0 0 + + + + + 1 1 1 + + + 10 + + + + + + + + + + + + + + + + + 1 1 1 + + + + + + + 0 + 0 + 0 + + + + + -0.170626 -11.744 24.2897 0 1.1458 1.5962 + orbit + perspective + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/world/room/model.config b/chapt8/chapt8_ws/src/fishbot_description/world/room/model.config new file mode 100644 index 0000000..6222143 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/world/room/model.config @@ -0,0 +1,11 @@ + + + room + 1.0 + model.sdf + + + + + + diff --git a/chapt8/chapt8_ws/src/fishbot_description/world/room/model.sdf b/chapt8/chapt8_ws/src/fishbot_description/world/room/model.sdf new file mode 100644 index 0000000..d499393 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_description/world/room/model.sdf @@ -0,0 +1,297 @@ + + + + -0.972666 -0.903877 0 0 -0 0 + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 5.50955 0 0 -0 0 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 9.43892 -0.040455 0 0 -0 -1.5708 + + + + + + 19 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 19 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.013921 -5.59046 0 0 -0 3.14159 + + + + + + 11.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 11.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -9.41108 -0.040455 0 0 -0 1.5708 + + + + + + 4.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 4.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -1.11769 3.29046 0 0 -0 -1.5708 + + + + + + 6.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -4.41769 0.990456 0 0 -0 3.14159 + + + + + + 3.47722 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 3.47722 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 0.391784 -3.86799 0 0 -0 1.5708 + + + + + + 7.5 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 7.5 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + 4.06678 -2.24484 0 0 -0 0 + + + + + + 6.25 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 6.25 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -6.38892 -1.80969 0 0 -0 0 + + + + + + 2.75 0.15 2.5 + + + 0 0 1.25 0 -0 0 + + + 0 0 1.25 0 -0 0 + + + 2.75 0.15 2.5 + + + + + 1 1 1 1 + + + 0 + + + -3.33892 -3.10969 0 0 -0 -1.5708 + + 1 + + diff --git a/chapt8/chapt8_ws/src/fishbot_navigation2/CMakeLists.txt b/chapt8/chapt8_ws/src/fishbot_navigation2/CMakeLists.txt new file mode 100644 index 0000000..c185e3e --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_navigation2/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_navigation2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY launch maps config + DESTINATION share/${PROJECT_NAME} +) + + +ament_package() diff --git a/chapt8/chapt8_ws/src/fishbot_navigation2/config/nav2_params.yaml b/chapt8/chapt8_ws/src/fishbot_navigation2/config/nav2_params.yaml new file mode 100644 index 0000000..a15be1e --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_navigation2/config/nav2_params.yaml @@ -0,0 +1,317 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "nav2_custom_controller::CustomController" + max_linear_speed: 0.1 + max_angular_speed: 1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + # GridBased: + # plugin: "nav2_navfn_planner/NavfnPlanner" + # tolerance: 0.5 + # use_astar: false + # allow_unknown: true + # plugins: ["GridBased"] + # use_sim_time: True + GridBased: + plugin: "nav2_custom_planner/CustomPlanner" + interpolation_resolution: 0.1 + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/chapt8/chapt8_ws/src/fishbot_navigation2/launch/navigation2.launch.py b/chapt8/chapt8_ws/src/fishbot_navigation2/launch/navigation2.launch.py new file mode 100644 index 0000000..a55c3d5 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_navigation2/launch/navigation2.launch.py @@ -0,0 +1,49 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # 获取与拼接默认路径 + fishbot_navigation2_dir = get_package_share_directory( + 'fishbot_navigation2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + rviz_config_dir = os.path.join( + nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz') + + # 创建 Launch 配置 + use_sim_time = launch.substitutions.LaunchConfiguration( + 'use_sim_time', default='true') + map_yaml_path = launch.substitutions.LaunchConfiguration( + 'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml')) + nav2_param_path = launch.substitutions.LaunchConfiguration( + 'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml')) + + return launch.LaunchDescription([ + # 声明新的 Launch 参数 + launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time, + description='Use simulation (Gazebo) clock if true'), + launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path, + description='Full path to map file to load'), + launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path, + description='Full path to param file to load'), + + launch.actions.IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/bringup_launch.py']), + # 使用 Launch 参数替换原有参数 + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path}.items(), + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + parameters=[{'use_sim_time': use_sim_time}], + output='screen'), + ]) \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/fishbot_navigation2/maps/room.pgm b/chapt8/chapt8_ws/src/fishbot_navigation2/maps/room.pgm new file mode 100644 index 0000000..6bdfd22 Binary files /dev/null and b/chapt8/chapt8_ws/src/fishbot_navigation2/maps/room.pgm differ diff --git a/chapt8/chapt8_ws/src/fishbot_navigation2/maps/room.yaml b/chapt8/chapt8_ws/src/fishbot_navigation2/maps/room.yaml new file mode 100644 index 0000000..d0cbed8 --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_navigation2/maps/room.yaml @@ -0,0 +1,7 @@ +image: room.pgm +mode: trinary +resolution: 0.05 +origin: [-10.4, -6.53, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/fishbot_navigation2/package.xml b/chapt8/chapt8_ws/src/fishbot_navigation2/package.xml new file mode 100644 index 0000000..19065ec --- /dev/null +++ b/chapt8/chapt8_ws/src/fishbot_navigation2/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_navigation2 + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt8/chapt8_ws/src/nav2_custom_controller/CMakeLists.txt b/chapt8/chapt8_ws/src/nav2_custom_controller/CMakeLists.txt new file mode 100644 index 0000000..380a549 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_controller/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_custom_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(nav2_core REQUIRED) +find_package(pluginlib REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# 包含头文件目录 +include_directories(include) +# 定义库名称 +set(library_name ${PROJECT_NAME}_plugin) +# 创建共享库 +add_library(${library_name} SHARED src/custom_controller.cpp) +# 指定库的依赖关系 +ament_target_dependencies(${library_name} nav2_core pluginlib) +# 安装库文件到指定目录 +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +# 安装头文件到指定目录 +install(DIRECTORY include/ + DESTINATION include/) +# 导出插件描述文件 +pluginlib_export_plugin_description_file(nav2_core nav2_custom_controller.xml) + +ament_package() diff --git a/chapt8/chapt8_ws/src/nav2_custom_controller/include/nav2_custom_controller/custom_controller.hpp b/chapt8/chapt8_ws/src/nav2_custom_controller/include/nav2_custom_controller/custom_controller.hpp new file mode 100644 index 0000000..599cb2d --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_controller/include/nav2_custom_controller/custom_controller.hpp @@ -0,0 +1,62 @@ +#ifndef NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_ +#define NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_ + +#include +#include +#include + +#include "nav2_core/controller.hpp" +#include "rclcpp/rclcpp.hpp" +#include "nav2_util/robot_utils.hpp" + + +namespace nav2_custom_controller { + +class CustomController : public nav2_core::Controller { +public: + CustomController() = default; + ~CustomController() override = default; + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, + std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + void cleanup() override; + void activate() override; + void deactivate() override; + geometry_msgs::msg::TwistStamped + computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose, + const geometry_msgs::msg::Twist &velocity, + nav2_core::GoalChecker * goal_checker) override; + void setPlan(const nav_msgs::msg::Path &path) override; + void setSpeedLimit(const double &speed_limit, + const bool &percentage) override; + +protected: + // 存储插件名称 + std::string plugin_name_; + // 存储坐标变换缓存指针，可用于查询坐标关系 + std::shared_ptr tf_; + // 存储代价地图 + std::shared_ptr costmap_ros_; + // 存储节点指针 + nav2_util::LifecycleNode::SharedPtr node_; + // 存储全局代价地图 + nav2_costmap_2d::Costmap2D *costmap_; + // 存储 setPlan 提供的全局路径 + nav_msgs::msg::Path global_plan_; + // 参数：最大线速度角速度 + double max_angular_speed_; + double max_linear_speed_; + + // 获取路径中距离当前点最近的点 + geometry_msgs::msg::PoseStamped + getNearestTargetPose(const geometry_msgs::msg::PoseStamped ¤t_pose); + // 计算目标点方向和当前位置的角度差 + double + calculateAngleDifference(const geometry_msgs::msg::PoseStamped ¤t_pose, + const geometry_msgs::msg::PoseStamped &target_pose); +}; + +} // namespace nav2_custom_controller + +#endif // NAV2_CUSTOM_CONTROLLER__NAV2_CUSTOM_CONTROLLER_HPP_ \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/nav2_custom_controller/nav2_custom_controller.xml b/chapt8/chapt8_ws/src/nav2_custom_controller/nav2_custom_controller.xml new file mode 100644 index 0000000..06e97ef --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_controller/nav2_custom_controller.xml @@ -0,0 +1,9 @@ + + + + + 自定义导航控制器 + + + + \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/nav2_custom_controller/package.xml b/chapt8/chapt8_ws/src/nav2_custom_controller/package.xml new file mode 100644 index 0000000..84534e0 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_controller/package.xml @@ -0,0 +1,22 @@ + + + + nav2_custom_controller + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + nav2_core + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/chapt8/chapt8_ws/src/nav2_custom_controller/src/custom_controller.cpp b/chapt8/chapt8_ws/src/nav2_custom_controller/src/custom_controller.cpp new file mode 100644 index 0000000..541ea61 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_controller/src/custom_controller.cpp @@ -0,0 +1,142 @@ +#include "nav2_custom_controller/custom_controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include +#include +#include +#include +#include +#include + +namespace nav2_custom_controller { +void CustomController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, + std::shared_ptr tf, + std::shared_ptr costmap_ros) { + node_ = parent.lock(); + costmap_ros_ = costmap_ros; + tf_ = tf; + plugin_name_ = name; + + // 声明并获取参数，设置最大线速度和最大角速度 + nav2_util::declare_parameter_if_not_declared( + node_, plugin_name_ + ".max_linear_speed", rclcpp::ParameterValue(0.1)); + node_->get_parameter(plugin_name_ + ".max_linear_speed", max_linear_speed_); + nav2_util::declare_parameter_if_not_declared( + node_, plugin_name_ + ".max_angular_speed", rclcpp::ParameterValue(1.0)); + node_->get_parameter(plugin_name_ + ".max_angular_speed", max_angular_speed_); +} + +void CustomController::cleanup() { + RCLCPP_INFO(node_->get_logger(), + "清理控制器：%s 类型为 nav2_custom_controller::CustomController", + plugin_name_.c_str()); +} + +void CustomController::activate() { + RCLCPP_INFO(node_->get_logger(), + "激活控制器：%s 类型为 nav2_custom_controller::CustomController", + plugin_name_.c_str()); +} + +void CustomController::deactivate() { + RCLCPP_INFO(node_->get_logger(), + "停用控制器：%s 类型为 nav2_custom_controller::CustomController", + plugin_name_.c_str()); +} + +geometry_msgs::msg::TwistStamped CustomController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &pose, + const geometry_msgs::msg::Twist &, nav2_core::GoalChecker *) { + // 1. 检查路径是否为空 + if (global_plan_.poses.empty()) { + throw nav2_core::PlannerException("收到长度为零的路径"); + } + + // 2.将机器人当前姿态转换到全局计划坐标系中 + geometry_msgs::msg::PoseStamped pose_in_globalframe; + if (!nav2_util::transformPoseInTargetFrame( + pose, pose_in_globalframe, *tf_, global_plan_.header.frame_id, 0.1)) { + throw nav2_core::PlannerException("无法将机器人姿态转换为全局计划的坐标系"); + } + + // 3.获取最近的目标点和计算角度差 + auto target_pose = getNearestTargetPose(pose_in_globalframe); + auto angle_diff = calculateAngleDifference(pose_in_globalframe, target_pose); + + // 4.根据角度差计算线速度和角速度 + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header.frame_id = pose_in_globalframe.header.frame_id; + cmd_vel.header.stamp = node_->get_clock()->now(); + // 根据角度差计算速度，角度差大于 0.3 则原地旋转，否则直行 + if (fabs(angle_diff) > M_PI/10.0) { + cmd_vel.twist.linear.x = .0; + cmd_vel.twist.angular.z = fabs(angle_diff) / angle_diff * max_angular_speed_; + } else { + cmd_vel.twist.linear.x = max_linear_speed_; + cmd_vel.twist.angular.z = .0; + } + RCLCPP_INFO(node_->get_logger(), "控制器：%s 发送速度(%f,%f)", + plugin_name_.c_str(), cmd_vel.twist.linear.x, + cmd_vel.twist.angular.z); + return cmd_vel; +} + +void CustomController::setSpeedLimit(const double &speed_limit, + const bool &percentage) { + (void)percentage; + (void)speed_limit; +} + +void CustomController::setPlan(const nav_msgs::msg::Path &path) { + global_plan_ = path; +} + +geometry_msgs::msg::PoseStamped CustomController::getNearestTargetPose( + const geometry_msgs::msg::PoseStamped ¤t_pose) { + // 1.遍历路径获取路径中距离当前点最近的索引，存储到 nearest_pose_index + using nav2_util::geometry_utils::euclidean_distance; + int nearest_pose_index = 0; + double min_dist = euclidean_distance(current_pose, global_plan_.poses.at(0)); + for (unsigned int i = 1; i < global_plan_.poses.size(); i++) { + double dist = euclidean_distance(current_pose, global_plan_.poses.at(i)); + if (dist < min_dist) { + nearest_pose_index = i; + min_dist = dist; + } + } + // 2.从路径中擦除头部到最近点的路径 + global_plan_.poses.erase(std::begin(global_plan_.poses), + std::begin(global_plan_.poses) + nearest_pose_index); + // 3.如果只有一个点则直接，否则返回最近点的下一个点 + if (global_plan_.poses.size() == 1) { + return global_plan_.poses.at(0); + } + return global_plan_.poses.at(1); + return current_pose; +} + +double CustomController::calculateAngleDifference( + const geometry_msgs::msg::PoseStamped ¤t_pose, + const geometry_msgs::msg::PoseStamped &target_pose) { + // 计算当前姿态与目标姿态之间的角度差 + // 1. 获取当前角度 + float current_robot_yaw = tf2::getYaw(current_pose.pose.orientation); + // 2.获取目标点朝向 + float target_angle = + std::atan2(target_pose.pose.position.y - current_pose.pose.position.y, + target_pose.pose.position.x - current_pose.pose.position.x); + // 3.计算角度差，并转换到 -M_PI 到 M_PI 之间 + double angle_diff = target_angle - current_robot_yaw; + if (angle_diff < -M_PI) { + angle_diff += 2.0 * M_PI; + } else if (angle_diff > M_PI) { + angle_diff -= 2.0 * M_PI; + } + return angle_diff; +} +} // namespace nav2_custom_controller + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_custom_controller::CustomController,nav2_core::Controller) \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/nav2_custom_planner/CMakeLists.txt b/chapt8/chapt8_ws/src/nav2_custom_planner/CMakeLists.txt new file mode 100644 index 0000000..867e987 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_planner/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(nav2_custom_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(nav2_core REQUIRED) +find_package(pluginlib REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# 包含头文件目录 +include_directories(include) +# 定义库名称 +set(library_name ${PROJECT_NAME}_plugin) +# 创建共享库 +add_library(${library_name} SHARED src/nav2_custom_planner.cpp) +# 指定库的依赖关系 +ament_target_dependencies(${library_name} nav2_core pluginlib) +# 安装库文件到指定目录 +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) +# 安装头文件到指定目录 +install(DIRECTORY include/ + DESTINATION include/ ) +# 导出插件描述文件 +pluginlib_export_plugin_description_file(nav2_core custom_planner_plugin.xml) + +ament_package() diff --git a/chapt8/chapt8_ws/src/nav2_custom_planner/custom_planner_plugin.xml b/chapt8/chapt8_ws/src/nav2_custom_planner/custom_planner_plugin.xml new file mode 100644 index 0000000..57621e6 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_planner/custom_planner_plugin.xml @@ -0,0 +1,5 @@ + + + 是一个自定义示例插件，用于生成自定义路径。 + + \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/nav2_custom_planner/include/nav2_custom_planner/nav2_custom_planner.hpp b/chapt8/chapt8_ws/src/nav2_custom_planner/include/nav2_custom_planner/nav2_custom_planner.hpp new file mode 100644 index 0000000..c92ea49 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_planner/include/nav2_custom_planner/nav2_custom_planner.hpp @@ -0,0 +1,51 @@ +#ifndef NAV2_CUSTOM_PLANNER__NAV2_CUSTOM_PLANNER_HPP_ +#define NAV2_CUSTOM_PLANNER__NAV2_CUSTOM_PLANNER_HPP_ +#include +#include +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "nav2_core/global_planner.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav_msgs/msg/path.hpp" + +namespace nav2_custom_planner { +// 自定义导航规划器类 +class CustomPlanner : public nav2_core::GlobalPlanner { +public: + CustomPlanner() = default; + ~CustomPlanner() = default; + // 插件配置方法 + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, + std::shared_ptr tf, + std::shared_ptr costmap_ros) override; + // 插件清理方法 + void cleanup() override; + // 插件激活方法 + void activate() override; + // 插件停用方法 + void deactivate() override; + // 为给定的起始和目标位姿创建路径的方法 + nav_msgs::msg::Path + createPlan(const geometry_msgs::msg::PoseStamped &start, + const geometry_msgs::msg::PoseStamped &goal) override; + +private: + // 坐标变换缓存指针，可用于查询坐标关系 + std::shared_ptr tf_; + // 节点指针 + nav2_util::LifecycleNode::SharedPtr node_; + // 全局代价地图 + nav2_costmap_2d::Costmap2D *costmap_; + // 全局代价地图的坐标系 + std::string global_frame_, name_; + // 插值分辨率 + double interpolation_resolution_; +}; + +} // namespace nav2_custom_planner + +#endif // NAV2_CUSTOM_PLANNER__NAV2_CUSTOM_PLANNER_HPP_ \ No newline at end of file diff --git a/chapt8/chapt8_ws/src/nav2_custom_planner/package.xml b/chapt8/chapt8_ws/src/nav2_custom_planner/package.xml new file mode 100644 index 0000000..dbb4261 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_planner/package.xml @@ -0,0 +1,22 @@ + + + + nav2_custom_planner + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + nav2_core + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/chapt8/chapt8_ws/src/nav2_custom_planner/src/nav2_custom_planner.cpp b/chapt8/chapt8_ws/src/nav2_custom_planner/src/nav2_custom_planner.cpp new file mode 100644 index 0000000..8872a57 --- /dev/null +++ b/chapt8/chapt8_ws/src/nav2_custom_planner/src/nav2_custom_planner.cpp @@ -0,0 +1,126 @@ +#include "nav2_util/node_utils.hpp" +#include +#include +#include + +#include "nav2_core/exceptions.hpp" +#include "nav2_custom_planner/nav2_custom_planner.hpp" + +namespace nav2_custom_planner +{ + + void CustomPlanner::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, + std::shared_ptr tf, + std::shared_ptr costmap_ros) + { + tf_ = tf; + node_ = parent.lock(); + name_ = name; + costmap_ = costmap_ros->getCostmap(); + global_frame_ = costmap_ros->getGlobalFrameID(); + // 参数初始化 + nav2_util::declare_parameter_if_not_declared( + node_, name_ + ".interpolation_resolution", rclcpp::ParameterValue(0.1)); + node_->get_parameter(name_ + ".interpolation_resolution", + interpolation_resolution_); + } + + void CustomPlanner::cleanup() + { + RCLCPP_INFO(node_->get_logger(), "正在清理类型为 CustomPlanner 的插件 %s", + name_.c_str()); + } + + void CustomPlanner::activate() + { + RCLCPP_INFO(node_->get_logger(), "正在激活类型为 CustomPlanner 的插件 %s", + name_.c_str()); + } + + void CustomPlanner::deactivate() + { + RCLCPP_INFO(node_->get_logger(), "正在停用类型为 CustomPlanner 的插件 %s", + name_.c_str()); + } + + nav_msgs::msg::Path + CustomPlanner::createPlan(const geometry_msgs::msg::PoseStamped &start, + const geometry_msgs::msg::PoseStamped &goal) + { + // 1.声明并初始化 global_path + nav_msgs::msg::Path global_path; + global_path.poses.clear(); + global_path.header.stamp = node_->now(); + global_path.header.frame_id = global_frame_; + + // 2.检查目标和起始状态是否在全局坐标系中 + if (start.header.frame_id != global_frame_) + { + RCLCPP_ERROR(node_->get_logger(), "规划器仅接受来自 %s 坐标系的起始位置", + global_frame_.c_str()); + return global_path; + } + + if (goal.header.frame_id != global_frame_) + { + RCLCPP_INFO(node_->get_logger(), "规划器仅接受来自 %s 坐标系的目标位置", + global_frame_.c_str()); + return global_path; + } + + // 3.计算当前插值分辨率 interpolation_resolution_ 下的循环次数和步进值 + int total_number_of_loop = + std::hypot(goal.pose.position.x - start.pose.position.x, + goal.pose.position.y - start.pose.position.y) / + interpolation_resolution_; + double x_increment = + (goal.pose.position.x - start.pose.position.x) / total_number_of_loop; + double y_increment = + (goal.pose.position.y - start.pose.position.y) / total_number_of_loop; + + // 4. 生成路径 + for (int i = 0; i < total_number_of_loop; ++i) + { + geometry_msgs::msg::PoseStamped pose; // 生成一个点 + pose.pose.position.x = start.pose.position.x + x_increment * i; + pose.pose.position.y = start.pose.position.y + y_increment * i; + pose.pose.position.z = 0.0; + pose.header.stamp = node_->now(); + pose.header.frame_id = global_frame_; + // 将该点放到路径中 + global_path.poses.push_back(pose); + } + + // 5.使用 costmap 检查该条路径是否经过障碍物 + for (geometry_msgs::msg::PoseStamped pose : global_path.poses) + { + unsigned int mx, my; // 将点的坐标转换为栅格坐标 + if (costmap_->worldToMap(pose.pose.position.x, pose.pose.position.y, mx, my)) + { + unsigned char cost = costmap_->getCost(mx, my); // 获取对应栅格的代价值 + // 如果存在致命障碍物则抛出异常 + if (cost == nav2_costmap_2d::LETHAL_OBSTACLE) + { + RCLCPP_WARN(node_->get_logger(),"在(%f,%f)检测到致命障碍物，规划失败。", + pose.pose.position.x, pose.pose.position.y); + throw nav2_core::PlannerException( + "无法创建目标规划: " + std::to_string(goal.pose.position.x) + "," + + std::to_string(goal.pose.position.y)); + } + } + } + + // 6.收尾，将目标点作为路径的最后一个点并返回路径 + geometry_msgs::msg::PoseStamped goal_pose = goal; + goal_pose.header.stamp = node_->now(); + goal_pose.header.frame_id = global_frame_; + global_path.poses.push_back(goal_pose); + return global_path; + } + +} // namespace nav2_custom_planner + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_custom_planner::CustomPlanner, + nav2_core::GlobalPlanner) \ No newline at end of file diff --git a/chapt8/learn_pluginlib/src/motion_control_system/CMakeLists.txt b/chapt8/learn_pluginlib/src/motion_control_system/CMakeLists.txt new file mode 100644 index 0000000..2581b7a --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(motion_control_system) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +include_directories(include) +# ================添加库文件===================== +add_library(spin_motion_controller SHARED src/spin_motion_controller.cpp) +ament_target_dependencies(spin_motion_controller pluginlib ) + +add_executable(test_plugin src/test_plugin.cpp) +ament_target_dependencies(test_plugin pluginlib) +install(TARGETS test_plugin + DESTINATION lib/${PROJECT_NAME} +) + +install(TARGETS spin_motion_controller + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY include/ + DESTINATION include/ +) +# 导出插件描述文件 +pluginlib_export_plugin_description_file(motion_control_system spin_motion_plugins.xml) +ament_package() diff --git a/chapt8/learn_pluginlib/src/motion_control_system/LICENSE b/chapt8/learn_pluginlib/src/motion_control_system/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/LICENSE @@ -0,0 +1,202 @@ + + 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. diff --git a/chapt8/learn_pluginlib/src/motion_control_system/include/motion_control_system/motion_control_interface.hpp b/chapt8/learn_pluginlib/src/motion_control_system/include/motion_control_system/motion_control_interface.hpp new file mode 100644 index 0000000..97d4000 --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/include/motion_control_system/motion_control_interface.hpp @@ -0,0 +1,15 @@ +#ifndef MOTION_CONTROL_INTERFACE_HPP +#define MOTION_CONTROL_INTERFACE_HPP + +namespace motion_control_system { + +class MotionController { +public: + virtual void start() = 0; + virtual void stop() = 0; + virtual ~MotionController() {} +}; + +} // namespace motion_control_system + +#endif // MOTION_CONTROL_INTERFACE_HPP \ No newline at end of file diff --git a/chapt8/learn_pluginlib/src/motion_control_system/include/motion_control_system/spin_motion_controller.hpp b/chapt8/learn_pluginlib/src/motion_control_system/include/motion_control_system/spin_motion_controller.hpp new file mode 100644 index 0000000..028b707 --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/include/motion_control_system/spin_motion_controller.hpp @@ -0,0 +1,17 @@ +#ifndef SPIN_MOTION_CONTROLLER_HPP +#define SPIN_MOTION_CONTROLLER_HPP + +#include "motion_control_system/motion_control_interface.hpp" + +namespace motion_control_system +{ + class SpinMotionController : public MotionController + { + public: + void start() override; + void stop() override; + }; + +} // namespace motion_control_system + +#endif // SPIN_MOTION_CONTROLLER_HPP \ No newline at end of file diff --git a/chapt8/learn_pluginlib/src/motion_control_system/package.xml b/chapt8/learn_pluginlib/src/motion_control_system/package.xml new file mode 100644 index 0000000..a9d3c4b --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/package.xml @@ -0,0 +1,20 @@ + + + + motion_control_system + 0.0.0 + TODO: Package description + fishros + Apache-2.0 + + ament_cmake + + pluginlib + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt8/learn_pluginlib/src/motion_control_system/spin_motion_plugins.xml b/chapt8/learn_pluginlib/src/motion_control_system/spin_motion_plugins.xml new file mode 100644 index 0000000..5eff615 --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/spin_motion_plugins.xml @@ -0,0 +1,5 @@ + + + Spin Motion Controller + + \ No newline at end of file diff --git a/chapt8/learn_pluginlib/src/motion_control_system/src/spin_motion_controller.cpp b/chapt8/learn_pluginlib/src/motion_control_system/src/spin_motion_controller.cpp new file mode 100644 index 0000000..0af1c7d --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/src/spin_motion_controller.cpp @@ -0,0 +1,17 @@ +#include +#include "motion_control_system/spin_motion_controller.hpp" +namespace motion_control_system +{ + void SpinMotionController::start() + { + // 实现旋转运动控制逻辑 + std::cout << "SpinMotionController::start" << std::endl; + } + void SpinMotionController::stop() + { + // 停止运动控制 + std::cout << "SpinMotionController::stop" << std::endl; + } +} // namespace motion_control_system +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(motion_control_system::SpinMotionController, motion_control_system::MotionController) \ No newline at end of file diff --git a/chapt8/learn_pluginlib/src/motion_control_system/src/test_plugin.cpp b/chapt8/learn_pluginlib/src/motion_control_system/src/test_plugin.cpp new file mode 100644 index 0000000..29c9e0f --- /dev/null +++ b/chapt8/learn_pluginlib/src/motion_control_system/src/test_plugin.cpp @@ -0,0 +1,20 @@ +#include "motion_control_system/motion_control_interface.hpp" +#include + +int main(int argc, char **argv) { + // 判断参数数量是否合法 + if (argc != 2) + return 0; + // 通过命令行参数，选择要加载的插件,argv[0]是可执行文件名，argv[1]表示参数名 + std::string controller_name = argv[1]; + // 1.通过功能包名称和基类名称创建控制器加载器 + pluginlib::ClassLoader + controller_loader("motion_control_system", + "motion_control_system::MotionController"); + // 2.使用加载器加载指定名称的插件，返回的是指定插件类的对象的指针 + auto controller = controller_loader.createSharedInstance(controller_name); + // 3.调用插件的方法 + controller->start(); + controller->stop(); + return 0; +} \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt b/chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt new file mode 100644 index 0000000..a3efba1 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_bringup/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +add_executable(odom2tf src/odom2tf.cpp) +ament_target_dependencies(odom2tf + rclcpp tf2 nav_msgs geometry_msgs tf2_ros +) +install(TARGETS odom2tf + DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) +ament_package() diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py b/chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py new file mode 100644 index 0000000..71fffdd --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_bringup/launch/bringup.launch.py @@ -0,0 +1,50 @@ +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + +def generate_launch_description(): + fishbot_bringup_dir = get_package_share_directory( + 'fishbot_bringup') + ydlidar_ros2_dir = get_package_share_directory( + 'ydlidar') + + urdf2tf = launch.actions.IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [fishbot_bringup_dir, '/launch', '/urdf2tf.launch.py']), + ) + + odom2tf = launch_ros.actions.Node( + package='fishbot_bringup', + executable='odom2tf', + output='screen' + ) + + microros_agent = launch_ros.actions.Node( + package='micro_ros_agent', + executable='micro_ros_agent', + arguments=['udp4','--port','8888'], + output='screen' + ) + + ros_serail2wifi = launch_ros.actions.Node( + package='ros_serail2wifi', + executable='tcp_server', + parameters=[{'serial_port': '/tmp/tty_laser'}], + output='screen' + ) + + ydlidar = launch.actions.IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ydlidar_ros2_dir, '/launch', '/ydlidar_launch.py']), + ) + + # 使用 TimerAction 启动后 5 秒执行 ydlidar 节点 + ydlidar_delay = launch.actions.TimerAction(period=5.0, actions=[ydlidar]) + return launch.LaunchDescription([ + urdf2tf, + odom2tf, + microros_agent, + ros_serail2wifi, + ydlidar_delay + ]) \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/launch/urdf2tf.launch.py b/chapt9/fishbot_ws/src/fishbot_bringup/launch/urdf2tf.launch.py new file mode 100644 index 0000000..3c1e76e --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_bringup/launch/urdf2tf.launch.py @@ -0,0 +1,33 @@ +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # 获取默认路径 + urdf_tutorial_path = get_package_share_directory('fishbot_description') + fishbot_model_path = urdf_tutorial_path + '/urdf/fishbot.urdf' + # 为 Launch 声明参数 + action_declare_arg_mode_path = launch.actions.DeclareLaunchArgument( + name='model', default_value=str(fishbot_model_path), + description='URDF 的绝对路径') + # 获取文件内容生成新的参数 + robot_description = launch_ros.parameter_descriptions.ParameterValue( + launch.substitutions.Command( + ['cat ', launch.substitutions.LaunchConfiguration('model')]), + value_type=str) + # 状态发布节点 + robot_state_publisher_node = launch_ros.actions.Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': robot_description}] + ) + # 关节状态发布节点 + joint_state_publisher_node = launch_ros.actions.Node( + package='joint_state_publisher', + executable='joint_state_publisher', + ) + return launch.LaunchDescription([ + action_declare_arg_mode_path, joint_state_publisher_node, + robot_state_publisher_node, + ]) \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/package.xml b/chapt9/fishbot_ws/src/fishbot_bringup/package.xml new file mode 100644 index 0000000..8f7d5d7 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_bringup/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_bringup + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt9/fishbot_ws/src/fishbot_bringup/src/odom2tf.cpp b/chapt9/fishbot_ws/src/fishbot_bringup/src/odom2tf.cpp new file mode 100644 index 0000000..310eac6 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_bringup/src/odom2tf.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include +#include + +class OdomTopic2TF : public rclcpp::Node { +public: + OdomTopic2TF(std::string name) : Node(name) { + // 创建 odom 话题订阅者，使用传感器数据的 Qos + odom_subscribe_ = this->create_subscription( + "odom", rclcpp::SensorDataQoS(), + std::bind(&OdomTopic2TF::odom_callback_, this, std::placeholders::_1)); + // 创建一个tf2_ros::TransformBroadcaster用于广播坐标变换 + tf_broadcaster_ = std::make_unique(this); + } + +private: + rclcpp::Subscription::SharedPtr odom_subscribe_; + std::unique_ptr tf_broadcaster_; + // 回调函数，处理接收到的odom消息，并发布tf + void odom_callback_(const nav_msgs::msg::Odometry::SharedPtr msg) { + geometry_msgs::msg::TransformStamped transform; + transform.header = msg->header; // 使用消息的时间戳和框架ID + transform.child_frame_id = msg->child_frame_id; + transform.transform.translation.x = msg->pose.pose.position.x; + transform.transform.translation.y = msg->pose.pose.position.y; + transform.transform.translation.z = msg->pose.pose.position.z; + transform.transform.rotation.x = msg->pose.pose.orientation.x; + transform.transform.rotation.y = msg->pose.pose.orientation.y; + transform.transform.rotation.z = msg->pose.pose.orientation.z; + transform.transform.rotation.w = msg->pose.pose.orientation.w; + // 广播坐标变换信息 + tf_broadcaster_->sendTransform(transform); + }; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared("odom2tf"); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt b/chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt new file mode 100644 index 0000000..80f85e1 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_description/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_description) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + urdf + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/chapt9/fishbot_ws/src/fishbot_description/package.xml b/chapt9/fishbot_ws/src/fishbot_description/package.xml new file mode 100644 index 0000000..15178d3 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_description/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_description + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf b/chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf new file mode 100644 index 0000000..9ebe729 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_description/urdf/fishbot.urdf @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt b/chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt new file mode 100644 index 0000000..2310073 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_navigation2/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(fishbot_navigation2) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + launch + config + maps + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml b/chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml new file mode 100644 index 0000000..5f4b08f --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_navigation2/config/nav2_params.yaml @@ -0,0 +1,349 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/launch/navigation2.launch.py b/chapt9/fishbot_ws/src/fishbot_navigation2/launch/navigation2.launch.py new file mode 100644 index 0000000..a55c3d5 --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_navigation2/launch/navigation2.launch.py @@ -0,0 +1,49 @@ +import os +import launch +import launch_ros +from ament_index_python.packages import get_package_share_directory +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + # 获取与拼接默认路径 + fishbot_navigation2_dir = get_package_share_directory( + 'fishbot_navigation2') + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + rviz_config_dir = os.path.join( + nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz') + + # 创建 Launch 配置 + use_sim_time = launch.substitutions.LaunchConfiguration( + 'use_sim_time', default='true') + map_yaml_path = launch.substitutions.LaunchConfiguration( + 'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml')) + nav2_param_path = launch.substitutions.LaunchConfiguration( + 'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml')) + + return launch.LaunchDescription([ + # 声明新的 Launch 参数 + launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time, + description='Use simulation (Gazebo) clock if true'), + launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path, + description='Full path to map file to load'), + launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path, + description='Full path to param file to load'), + + launch.actions.IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [nav2_bringup_dir, '/launch', '/bringup_launch.py']), + # 使用 Launch 参数替换原有参数 + launch_arguments={ + 'map': map_yaml_path, + 'use_sim_time': use_sim_time, + 'params_file': nav2_param_path}.items(), + ), + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + parameters=[{'use_sim_time': use_sim_time}], + output='screen'), + ]) \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.pgm b/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.pgm new file mode 100644 index 0000000..4af93a9 Binary files /dev/null and b/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.pgm differ diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.yaml b/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.yaml new file mode 100644 index 0000000..cad4a2a --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_navigation2/maps/room.yaml @@ -0,0 +1,7 @@ +image: room.pgm +mode: trinary +resolution: 0.05 +origin: [-3.9, -1.82, 0] +negate: 0 +occupied_thresh: 0.65 +free_thresh: 0.25 \ No newline at end of file diff --git a/chapt9/fishbot_ws/src/fishbot_navigation2/package.xml b/chapt9/fishbot_ws/src/fishbot_navigation2/package.xml new file mode 100644 index 0000000..19065ec --- /dev/null +++ b/chapt9/fishbot_ws/src/fishbot_navigation2/package.xml @@ -0,0 +1,18 @@ + + + + fishbot_navigation2 + 0.0.0 + TODO: Package description + fishros + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + +