diff --git a/.github/workflows/auto_documentation.yml b/.github/workflows/auto_documentation.yml index 2318d5f4d..2f4b90477 100644 --- a/.github/workflows/auto_documentation.yml +++ b/.github/workflows/auto_documentation.yml @@ -29,8 +29,8 @@ jobs: submodules: recursive - name: Move directory run: | - mkdir -p $HOME/catkin_ws/src/soccerbot - mv $GITHUB_WORKSPACE/* $HOME/catkin_ws/src/soccerbot + mkdir -p $HOME/ros2_ws/src/soccerbot + mv $GITHUB_WORKSPACE/* $HOME/ros2_ws/src/soccerbot - name: Install apt fast run: | sudo apt-get update @@ -43,30 +43,30 @@ jobs: sudo apt-get clean - name: Install ros dependencies run: | - cd $HOME/catkin_ws/src/soccerbot + cd $HOME/ros2_ws/src/soccerbot source /opt/ros/noetic/setup.bash rosdep update - rosdep install --from-paths . --ignore-src -r -s | grep 'apt-get install' | awk '{print $5}' | sort > /tmp/catkin_install_list - sudo apt-fast install -y --no-install-recommends $(cat /tmp/catkin_install_list) - udo apt-fast install -y python3-catkin-tools python-is-python3 + rosdep install --from-paths . --ignore-src -r -s | grep 'apt-get install' | awk '{print $5}' | sort > /tmp/ros2_install_list + sudo apt-fast install -y --no-install-recommends $(cat /tmp/ros2_install_list) + udo apt-fast install -y python3-ros2-tools python-is-python3 - name: Install python dependencies run: | - cd $HOME/catkin_ws/src/soccerbot + cd $HOME/ros2_ws/src/soccerbot source /opt/ros/noetic/setup.bash pip3 install pyOpenSSL==23.0.0 pip3 install --upgrade pip pip3 install -r requirements.txt -f https://download.pytorch.org/whl/torch/ -f https://download.pytorch.org/whl/torchvision/ - name: Build Soccerbot run: | - cd $HOME/catkin_ws + cd $HOME/ros2_ws source /opt/ros/noetic/setup.bash - catkin init - catkin build + ros2 init + ros2 build - name: Build Sphinx Documentation run: | - cd $HOME/catkin_ws/src/soccerbot/docs + cd $HOME/ros2_ws/src/soccerbot/docs source /opt/ros/noetic/setup.bash - source $HOME/catkin_ws/devel/setup.bash + source $HOME/ros2_ws/devel/setup.bash pip install -r requirements.txt roscore & make html @@ -75,7 +75,7 @@ jobs: - name: Upload artifact uses: actions/upload-pages-artifact@v2 with: - path: /home/runner/catkin_ws/src/soccerbot/docs/_build/html + path: /home/runner/ros2_ws/src/soccerbot/docs/_build/html - name: Setup tmate session if: ${{ failure() }} uses: mxschmitt/action-tmate@v3 diff --git a/.github/workflows/test_catkin_lint.yml b/.github/workflows/test_catkin_lint.yml index 9a5ee2633..aa2e1cb51 100644 --- a/.github/workflows/test_catkin_lint.yml +++ b/.github/workflows/test_catkin_lint.yml @@ -22,8 +22,8 @@ jobs: with: submodules: recursive - - name: Install catkin_lint - run: sudo apt update && sudo apt install catkin-lint + - name: Install ros2_lint + run: sudo apt update && sudo apt install ros2-lint - - name: Run catkin_lint - run: catkin_lint ${{github.workspace}} + - name: Run ros2_lint + run: ros2_lint ${{github.workspace}} diff --git a/.github/workflows/test_pytest.yml b/.github/workflows/test_pytest.yml index 8ad3ae93e..4b072166c 100644 --- a/.github/workflows/test_pytest.yml +++ b/.github/workflows/test_pytest.yml @@ -28,8 +28,8 @@ jobs: submodules: recursive - name: Move directory run: | - mkdir -p $HOME/catkin_ws/src/soccerbot - mv $GITHUB_WORKSPACE/* $HOME/catkin_ws/src/soccerbot + mkdir -p $HOME/ros2_ws/src/soccerbot + mv $GITHUB_WORKSPACE/* $HOME/ros2_ws/src/soccerbot - name: Install apt fast run: | sudo apt-get update @@ -42,25 +42,25 @@ jobs: sudo apt-get clean - name: Install ros dependencies run: | - cd $HOME/catkin_ws/src/soccerbot + cd $HOME/ros2_ws/src/soccerbot source /opt/ros/noetic/setup.bash rosdep update - rosdep install --from-paths . --ignore-src -r -s | grep 'apt-get install' | awk '{print $5}' | sort > /tmp/catkin_install_list - sudo apt-fast install -y --no-install-recommends $(cat /tmp/catkin_install_list) - sudo apt-fast install -y python3-catkin-tools python-is-python3 + rosdep install --from-paths . --ignore-src -r -s | grep 'apt-get install' | awk '{print $5}' | sort > /tmp/ros2_install_list + sudo apt-fast install -y --no-install-recommends $(cat /tmp/ros2_install_list) + sudo apt-fast install -y python3-ros2-tools python-is-python3 - name: Install python dependencies run: | - cd $HOME/catkin_ws/src/soccerbot + cd $HOME/ros2_ws/src/soccerbot source /opt/ros/noetic/setup.bash pip install pyOpenSSL==23.0.0 python3 -m pip install -U --user pip pip install -r tools/setup/requirements.txt - name: Build Soccerbot run: | - cd $HOME/catkin_ws + cd $HOME/ros2_ws source /opt/ros/noetic/setup.bash - catkin init - catkin build + ros2 init + ros2 build - name: Initialize fake display and roscore run: | source /opt/ros/noetic/setup.bash @@ -71,9 +71,9 @@ jobs: - name: Pytest run: | mkdir -p /home/$USER/.ros/config && cd /home/$USER/.ros/config && ln -s /opt/ros/noetic/etc/ros/python_logging.conf - cd $HOME/catkin_ws/src/soccerbot + cd $HOME/ros2_ws/src/soccerbot source /opt/ros/noetic/setup.bash - source $HOME/catkin_ws/devel/setup.bash + source $HOME/ros2_ws/devel/setup.bash export PYTHONPATH=$PYTHONPATH:soccer_common/src:soccer_pycontrol/src pytest -s --cache-clear --ignore=soccer_strategy --ignore=external --ignore=soccer_hardware/ --ignore=test_integration.py --ignore=soccer_webots diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 000000000..13566b81b --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Editor-based HTTP Client requests +/httpRequests/ +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml deleted file mode 100644 index 4175bbe75..000000000 --- a/.idea/inspectionProfiles/Project_Default.xml +++ /dev/null @@ -1,37 +0,0 @@ - - - - \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index e557d17a7..9de286525 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,7 +1,7 @@ - - + \ No newline at end of file diff --git a/.idea/soccerbot.iml b/.idea/soccerbot.iml index 5796eb0e1..d64a935ad 100644 --- a/.idea/soccerbot.iml +++ b/.idea/soccerbot.iml @@ -2,17 +2,21 @@ + - - - + + + + + + - + diff --git a/.idea/workspace.xml b/.idea/workspace.xml deleted file mode 100644 index cb46f465d..000000000 --- a/.idea/workspace.xml +++ /dev/null @@ -1,280 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - { - "associatedIndex": 6 -} - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1728503058115 - - - - - - - - - - - - file://$PROJECT_DIR$/soccer_control/soccer_pycontrol/test/test_pybullet.py - 29 - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index d0cd881f5..e743bb8fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -# toplevel CMakeLists.txt for a catkin workspace catkin/cmake/toplevel.cmake +# toplevel CMakeLists.txt for a ros2 workspace ros2/cmake/toplevel.cmake cmake_minimum_required(VERSION 3.0.2) @@ -11,9 +11,9 @@ set(CMAKE_PREFIX_PATH /opt/ros/noetic ) -# search for catkin within the workspace +# search for ros2 within the workspace set(_cmd - "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}" + "ros2_find_pkg" "ros2" "${CMAKE_SOURCE_DIR}" ) execute_process( COMMAND ${_cmd} @@ -23,19 +23,19 @@ execute_process( OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_STRIP_TRAILING_WHITESPACE ) if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) - # searching fot catkin resulted in an error + # searching fot ros2 resulted in an error string(REPLACE ";" " " _cmd_str "${_cmd}") message( FATAL_ERROR - "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}" + "Search for 'ros2' in workspace failed (${_cmd_str}): ${_err}" ) endif() -# include catkin from workspace or via find_package() +# include ros2 from workspace or via find_package() if(_res EQUAL 0) - set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") + set(ros2_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") # include all.cmake without add_subdirectory to let it operate in same scope - include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) + include(${ros2_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) add_subdirectory("${_out}") else() @@ -53,31 +53,31 @@ else() endif() endif() - # list of catkin workspaces - set(catkin_search_path "") + # list of ros2 workspaces + set(ros2_search_path "") foreach(path ${CMAKE_PREFIX_PATH}) - if(EXISTS "${path}/.catkin") - list(FIND catkin_search_path ${path} _index) + if(EXISTS "${path}/.ros2") + list(FIND ros2_search_path ${path} _index) if(_index EQUAL -1) - list(APPEND catkin_search_path ${path}) + list(APPEND ros2_search_path ${path}) endif() endif() endforeach() - # search for catkin in all workspaces + # search for ros2 in all workspaces set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) find_package( - catkin QUIET NO_POLICY_SCOPE PATHS ${catkin_search_path} + ros2 QUIET NO_POLICY_SCOPE PATHS ${ros2_search_path} NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH ) unset(CATKIN_TOPLEVEL_FIND_PACKAGE) - if(NOT catkin_FOUND) + if(NOT ros2_FOUND) message( FATAL_ERROR - "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before." + "find_package(ros2) failed. ros2 was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before." ) endif() endif() -catkin_workspace() +ros2_workspace() diff --git a/README.md b/README.md index 7e2a549aa..47978f6b8 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ https://github.com/utra-robosoccer/soccerbot/wiki/Onboarding roslaunch soccerbot sensors.launch __ns:=robot1 ``` -cd ~/catkin_ws +cd ~/ros2_ws source devel/setup.bash pytest src/soccerbot/soccer_trajectories/src/soccer_trajectories/test_trajectory.py::TestTrajectory::test_fixed_angles_trajectories @@ -42,7 +42,7 @@ pytest src/soccerbot/soccer_trajectories/src/soccer_trajectories/test_trajectory pytest src/soccerbot/soccer_pycontrol/src/soccer_pycontrol/test_walking.py::TestWalking::test_walk_1_real_robot -cd ~/catkin_ws +cd ~/ros2_ws ```bash source devel/setup.bash diff --git a/build/.built_by b/build/.built_by new file mode 100644 index 000000000..06e74acb6 --- /dev/null +++ b/build/.built_by @@ -0,0 +1 @@ +colcon diff --git a/build/COLCON_IGNORE b/build/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/build/Project/.cmake/api/v1/query/client-colcon-cmake/codemodel-v2 b/build/Project/.cmake/api/v1/query/client-colcon-cmake/codemodel-v2 new file mode 100644 index 000000000..e69de29bb diff --git a/build/Project/CMakeCache.txt b/build/Project/CMakeCache.txt new file mode 100644 index 000000000..a4fb2ea2f --- /dev/null +++ b/build/Project/CMakeCache.txt @@ -0,0 +1,369 @@ +# This is the CMakeCache file. +# For build in directory: /home/nazmus/ros2_ws/src/soccerbot/build/Project +# It was generated by CMake: /usr/bin/cmake +# You can edit this file to change values found and used by cmake. +# If you do not want to change any of the values, simply exit the editor. +# If you do want to change a value, simply edit, save, and exit the editor. +# The syntax for the file is as follows: +# KEY:TYPE=VALUE +# KEY is the name of a variable in the cache. +# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!. +# VALUE is the current value for the KEY. + +######################## +# EXTERNAL cache entries +######################## + +//Path to a program. +CMAKE_ADDR2LINE:FILEPATH=/usr/bin/addr2line + +//Path to a program. +CMAKE_AR:FILEPATH=/usr/bin/ar + +//Choose the type of build, options are: None Debug Release RelWithDebInfo +// MinSizeRel ... +CMAKE_BUILD_TYPE:STRING= + +//Enable/Disable color output during build. +CMAKE_COLOR_MAKEFILE:BOOL=ON + +//CXX compiler +CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++ + +//A wrapper around 'ar' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_CXX_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-11 + +//A wrapper around 'ranlib' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_CXX_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-11 + +//Flags used by the CXX compiler during all build types. +CMAKE_CXX_FLAGS:STRING= + +//Flags used by the CXX compiler during DEBUG builds. +CMAKE_CXX_FLAGS_DEBUG:STRING=-g + +//Flags used by the CXX compiler during MINSIZEREL builds. +CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the CXX compiler during RELEASE builds. +CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the CXX compiler during RELWITHDEBINFO builds. +CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//C compiler +CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc + +//A wrapper around 'ar' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_C_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-11 + +//A wrapper around 'ranlib' adding the appropriate '--plugin' option +// for the GCC compiler +CMAKE_C_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-11 + +//Flags used by the C compiler during all build types. +CMAKE_C_FLAGS:STRING= + +//Flags used by the C compiler during DEBUG builds. +CMAKE_C_FLAGS_DEBUG:STRING=-g + +//Flags used by the C compiler during MINSIZEREL builds. +CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG + +//Flags used by the C compiler during RELEASE builds. +CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG + +//Flags used by the C compiler during RELWITHDEBINFO builds. +CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG + +//Path to a program. +CMAKE_DLLTOOL:FILEPATH=CMAKE_DLLTOOL-NOTFOUND + +//Flags used by the linker during all build types. +CMAKE_EXE_LINKER_FLAGS:STRING= + +//Flags used by the linker during DEBUG builds. +CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during MINSIZEREL builds. +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during RELEASE builds. +CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during RELWITHDEBINFO builds. +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Enable/Disable output of compile commands during generation. +CMAKE_EXPORT_COMPILE_COMMANDS:BOOL= + +//Install path prefix, prepended onto install directories. +CMAKE_INSTALL_PREFIX:PATH=/home/nazmus/ros2_ws/src/soccerbot/install/Project + +//Path to a program. +CMAKE_LINKER:FILEPATH=/usr/bin/ld + +//Path to a program. +CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/gmake + +//Flags used by the linker during the creation of modules during +// all build types. +CMAKE_MODULE_LINKER_FLAGS:STRING= + +//Flags used by the linker during the creation of modules during +// DEBUG builds. +CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during the creation of modules during +// MINSIZEREL builds. +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during the creation of modules during +// RELEASE builds. +CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during the creation of modules during +// RELWITHDEBINFO builds. +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_NM:FILEPATH=/usr/bin/nm + +//Path to a program. +CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy + +//Path to a program. +CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump + +//Value Computed by CMake +CMAKE_PROJECT_DESCRIPTION:STATIC= + +//Value Computed by CMake +CMAKE_PROJECT_HOMEPAGE_URL:STATIC= + +//Value Computed by CMake +CMAKE_PROJECT_NAME:STATIC=Project + +//Path to a program. +CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib + +//Path to a program. +CMAKE_READELF:FILEPATH=/usr/bin/readelf + +//Flags used by the linker during the creation of shared libraries +// during all build types. +CMAKE_SHARED_LINKER_FLAGS:STRING= + +//Flags used by the linker during the creation of shared libraries +// during DEBUG builds. +CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during the creation of shared libraries +// during MINSIZEREL builds. +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during the creation of shared libraries +// during RELEASE builds. +CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during the creation of shared libraries +// during RELWITHDEBINFO builds. +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//If set, runtime paths are not added when installing shared libraries, +// but are added when building. +CMAKE_SKIP_INSTALL_RPATH:BOOL=NO + +//If set, runtime paths are not added when using shared libraries. +CMAKE_SKIP_RPATH:BOOL=NO + +//Flags used by the linker during the creation of static libraries +// during all build types. +CMAKE_STATIC_LINKER_FLAGS:STRING= + +//Flags used by the linker during the creation of static libraries +// during DEBUG builds. +CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING= + +//Flags used by the linker during the creation of static libraries +// during MINSIZEREL builds. +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING= + +//Flags used by the linker during the creation of static libraries +// during RELEASE builds. +CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING= + +//Flags used by the linker during the creation of static libraries +// during RELWITHDEBINFO builds. +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING= + +//Path to a program. +CMAKE_STRIP:FILEPATH=/usr/bin/strip + +//If this value is on, makefiles will be generated without the +// .SILENT directive, and all commands will be echoed to the console +// during the make. This is useful for debugging only. With Visual +// Studio IDE projects all commands are done without /nologo. +CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE + +//Value Computed by CMake +Project_BINARY_DIR:STATIC=/home/nazmus/ros2_ws/src/soccerbot/build/Project + +//Value Computed by CMake +Project_IS_TOP_LEVEL:STATIC=ON + +//Value Computed by CMake +Project_SOURCE_DIR:STATIC=/home/nazmus/ros2_ws/src/soccerbot + + +######################## +# INTERNAL cache entries +######################## + +//ADVANCED property for variable: CMAKE_ADDR2LINE +CMAKE_ADDR2LINE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_AR +CMAKE_AR-ADVANCED:INTERNAL=1 +//This is the directory where this CMakeCache.txt was created +CMAKE_CACHEFILE_DIR:INTERNAL=/home/nazmus/ros2_ws/src/soccerbot/build/Project +//Major version of cmake used to create the current loaded cache +CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3 +//Minor version of cmake used to create the current loaded cache +CMAKE_CACHE_MINOR_VERSION:INTERNAL=22 +//Patch version of cmake used to create the current loaded cache +CMAKE_CACHE_PATCH_VERSION:INTERNAL=1 +//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE +CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1 +//Path to CMake executable. +CMAKE_COMMAND:INTERNAL=/usr/bin/cmake +//Path to cpack program executable. +CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack +//Path to ctest program executable. +CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest +//ADVANCED property for variable: CMAKE_CXX_COMPILER +CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_COMPILER_AR +CMAKE_CXX_COMPILER_AR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_COMPILER_RANLIB +CMAKE_CXX_COMPILER_RANLIB-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS +CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG +CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL +CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE +CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO +CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER +CMAKE_C_COMPILER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER_AR +CMAKE_C_COMPILER_AR-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_COMPILER_RANLIB +CMAKE_C_COMPILER_RANLIB-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS +CMAKE_C_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG +CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL +CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE +CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO +CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_DLLTOOL +CMAKE_DLLTOOL-ADVANCED:INTERNAL=1 +//Executable file format +CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS +CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG +CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL +CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE +CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS +CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1 +//Name of external makefile project generator. +CMAKE_EXTRA_GENERATOR:INTERNAL= +//Name of generator. +CMAKE_GENERATOR:INTERNAL=Unix Makefiles +//Generator instance identifier. +CMAKE_GENERATOR_INSTANCE:INTERNAL= +//Name of generator platform. +CMAKE_GENERATOR_PLATFORM:INTERNAL= +//Name of generator toolset. +CMAKE_GENERATOR_TOOLSET:INTERNAL= +//Source directory with the top level CMakeLists.txt file for this +// project +CMAKE_HOME_DIRECTORY:INTERNAL=/home/nazmus/ros2_ws/src/soccerbot +//Install .so files without execute permission. +CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1 +//ADVANCED property for variable: CMAKE_LINKER +CMAKE_LINKER-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MAKE_PROGRAM +CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS +CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG +CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL +CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE +CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_NM +CMAKE_NM-ADVANCED:INTERNAL=1 +//number of local generators +CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=1 +//ADVANCED property for variable: CMAKE_OBJCOPY +CMAKE_OBJCOPY-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_OBJDUMP +CMAKE_OBJDUMP-ADVANCED:INTERNAL=1 +//Platform information initialized +CMAKE_PLATFORM_INFO_INITIALIZED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_RANLIB +CMAKE_RANLIB-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_READELF +CMAKE_READELF-ADVANCED:INTERNAL=1 +//Path to CMake installation. +CMAKE_ROOT:INTERNAL=/usr/share/cmake-3.22 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS +CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG +CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL +CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE +CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH +CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_SKIP_RPATH +CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS +CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG +CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL +CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE +CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO +CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1 +//ADVANCED property for variable: CMAKE_STRIP +CMAKE_STRIP-ADVANCED:INTERNAL=1 +//uname command +CMAKE_UNAME:INTERNAL=/usr/bin/uname +//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE +CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1 + diff --git a/build/Project/CMakeFiles/3.22.1/CMakeCCompiler.cmake b/build/Project/CMakeFiles/3.22.1/CMakeCCompiler.cmake new file mode 100644 index 000000000..488ad3751 --- /dev/null +++ b/build/Project/CMakeFiles/3.22.1/CMakeCCompiler.cmake @@ -0,0 +1,72 @@ +set(CMAKE_C_COMPILER "/usr/bin/cc") +set(CMAKE_C_COMPILER_ARG1 "") +set(CMAKE_C_COMPILER_ID "GNU") +set(CMAKE_C_COMPILER_VERSION "11.4.0") +set(CMAKE_C_COMPILER_VERSION_INTERNAL "") +set(CMAKE_C_COMPILER_WRAPPER "") +set(CMAKE_C_STANDARD_COMPUTED_DEFAULT "17") +set(CMAKE_C_EXTENSIONS_COMPUTED_DEFAULT "ON") +set(CMAKE_C_COMPILE_FEATURES "c_std_90;c_function_prototypes;c_std_99;c_restrict;c_variadic_macros;c_std_11;c_static_assert;c_std_17;c_std_23") +set(CMAKE_C90_COMPILE_FEATURES "c_std_90;c_function_prototypes") +set(CMAKE_C99_COMPILE_FEATURES "c_std_99;c_restrict;c_variadic_macros") +set(CMAKE_C11_COMPILE_FEATURES "c_std_11;c_static_assert") +set(CMAKE_C17_COMPILE_FEATURES "c_std_17") +set(CMAKE_C23_COMPILE_FEATURES "c_std_23") + +set(CMAKE_C_PLATFORM_ID "Linux") +set(CMAKE_C_SIMULATE_ID "") +set(CMAKE_C_COMPILER_FRONTEND_VARIANT "") +set(CMAKE_C_SIMULATE_VERSION "") + + + + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_C_COMPILER_AR "/usr/bin/gcc-ar-11") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_C_COMPILER_RANLIB "/usr/bin/gcc-ranlib-11") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_MT "") +set(CMAKE_COMPILER_IS_GNUCC 1) +set(CMAKE_C_COMPILER_LOADED 1) +set(CMAKE_C_COMPILER_WORKS TRUE) +set(CMAKE_C_ABI_COMPILED TRUE) + +set(CMAKE_C_COMPILER_ENV_VAR "CC") + +set(CMAKE_C_COMPILER_ID_RUN 1) +set(CMAKE_C_SOURCE_FILE_EXTENSIONS c;m) +set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) +set(CMAKE_C_LINKER_PREFERENCE 10) + +# Save compiler ABI information. +set(CMAKE_C_SIZEOF_DATA_PTR "8") +set(CMAKE_C_COMPILER_ABI "ELF") +set(CMAKE_C_BYTE_ORDER "LITTLE_ENDIAN") +set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_C_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_C_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") +endif() + +if(CMAKE_C_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + +set(CMAKE_C_CL_SHOWINCLUDES_PREFIX "") +if(CMAKE_C_CL_SHOWINCLUDES_PREFIX) + set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_C_CL_SHOWINCLUDES_PREFIX}") +endif() + + + + + +set(CMAKE_C_IMPLICIT_INCLUDE_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/11/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include") +set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "gcc;gcc_s;c;gcc;gcc_s") +set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/11;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") diff --git a/build/Project/CMakeFiles/3.22.1/CMakeCXXCompiler.cmake b/build/Project/CMakeFiles/3.22.1/CMakeCXXCompiler.cmake new file mode 100644 index 000000000..345e9307d --- /dev/null +++ b/build/Project/CMakeFiles/3.22.1/CMakeCXXCompiler.cmake @@ -0,0 +1,83 @@ +set(CMAKE_CXX_COMPILER "/usr/bin/c++") +set(CMAKE_CXX_COMPILER_ARG1 "") +set(CMAKE_CXX_COMPILER_ID "GNU") +set(CMAKE_CXX_COMPILER_VERSION "11.4.0") +set(CMAKE_CXX_COMPILER_VERSION_INTERNAL "") +set(CMAKE_CXX_COMPILER_WRAPPER "") +set(CMAKE_CXX_STANDARD_COMPUTED_DEFAULT "17") +set(CMAKE_CXX_EXTENSIONS_COMPUTED_DEFAULT "ON") +set(CMAKE_CXX_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters;cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates;cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates;cxx_std_17;cxx_std_20;cxx_std_23") +set(CMAKE_CXX98_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters") +set(CMAKE_CXX11_COMPILE_FEATURES "cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates") +set(CMAKE_CXX14_COMPILE_FEATURES "cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates") +set(CMAKE_CXX17_COMPILE_FEATURES "cxx_std_17") +set(CMAKE_CXX20_COMPILE_FEATURES "cxx_std_20") +set(CMAKE_CXX23_COMPILE_FEATURES "cxx_std_23") + +set(CMAKE_CXX_PLATFORM_ID "Linux") +set(CMAKE_CXX_SIMULATE_ID "") +set(CMAKE_CXX_COMPILER_FRONTEND_VARIANT "") +set(CMAKE_CXX_SIMULATE_VERSION "") + + + + +set(CMAKE_AR "/usr/bin/ar") +set(CMAKE_CXX_COMPILER_AR "/usr/bin/gcc-ar-11") +set(CMAKE_RANLIB "/usr/bin/ranlib") +set(CMAKE_CXX_COMPILER_RANLIB "/usr/bin/gcc-ranlib-11") +set(CMAKE_LINKER "/usr/bin/ld") +set(CMAKE_MT "") +set(CMAKE_COMPILER_IS_GNUCXX 1) +set(CMAKE_CXX_COMPILER_LOADED 1) +set(CMAKE_CXX_COMPILER_WORKS TRUE) +set(CMAKE_CXX_ABI_COMPILED TRUE) + +set(CMAKE_CXX_COMPILER_ENV_VAR "CXX") + +set(CMAKE_CXX_COMPILER_ID_RUN 1) +set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;m;mm;mpp;CPP;ixx;cppm) +set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC) + +foreach (lang C OBJC OBJCXX) + if (CMAKE_${lang}_COMPILER_ID_RUN) + foreach(extension IN LISTS CMAKE_${lang}_SOURCE_FILE_EXTENSIONS) + list(REMOVE_ITEM CMAKE_CXX_SOURCE_FILE_EXTENSIONS ${extension}) + endforeach() + endif() +endforeach() + +set(CMAKE_CXX_LINKER_PREFERENCE 30) +set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1) + +# Save compiler ABI information. +set(CMAKE_CXX_SIZEOF_DATA_PTR "8") +set(CMAKE_CXX_COMPILER_ABI "ELF") +set(CMAKE_CXX_BYTE_ORDER "LITTLE_ENDIAN") +set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") + +if(CMAKE_CXX_SIZEOF_DATA_PTR) + set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}") +endif() + +if(CMAKE_CXX_COMPILER_ABI) + set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}") +endif() + +if(CMAKE_CXX_LIBRARY_ARCHITECTURE) + set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") +endif() + +set(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX "") +if(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX) + set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_CXX_CL_SHOWINCLUDES_PREFIX}") +endif() + + + + + +set(CMAKE_CXX_IMPLICIT_INCLUDE_DIRECTORIES "/usr/include/c++/11;/usr/include/x86_64-linux-gnu/c++/11;/usr/include/c++/11/backward;/usr/lib/gcc/x86_64-linux-gnu/11/include;/usr/local/include;/usr/include/x86_64-linux-gnu;/usr/include") +set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;gcc_s;gcc;c;gcc_s;gcc") +set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/11;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") +set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") diff --git a/build/Project/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_C.bin b/build/Project/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_C.bin new file mode 100755 index 000000000..59672f585 Binary files /dev/null and b/build/Project/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_C.bin differ diff --git a/build/Project/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_CXX.bin b/build/Project/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_CXX.bin new file mode 100755 index 000000000..cfa527b53 Binary files /dev/null and b/build/Project/CMakeFiles/3.22.1/CMakeDetermineCompilerABI_CXX.bin differ diff --git a/build/Project/CMakeFiles/3.22.1/CMakeSystem.cmake b/build/Project/CMakeFiles/3.22.1/CMakeSystem.cmake new file mode 100644 index 000000000..3e9dcaeb9 --- /dev/null +++ b/build/Project/CMakeFiles/3.22.1/CMakeSystem.cmake @@ -0,0 +1,15 @@ +set(CMAKE_HOST_SYSTEM "Linux-6.8.0-60-generic") +set(CMAKE_HOST_SYSTEM_NAME "Linux") +set(CMAKE_HOST_SYSTEM_VERSION "6.8.0-60-generic") +set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") + + + +set(CMAKE_SYSTEM "Linux-6.8.0-60-generic") +set(CMAKE_SYSTEM_NAME "Linux") +set(CMAKE_SYSTEM_VERSION "6.8.0-60-generic") +set(CMAKE_SYSTEM_PROCESSOR "x86_64") + +set(CMAKE_CROSSCOMPILING "FALSE") + +set(CMAKE_SYSTEM_LOADED 1) diff --git a/build/Project/CMakeFiles/3.22.1/CompilerIdC/CMakeCCompilerId.c b/build/Project/CMakeFiles/3.22.1/CompilerIdC/CMakeCCompilerId.c new file mode 100644 index 000000000..41b99d778 --- /dev/null +++ b/build/Project/CMakeFiles/3.22.1/CompilerIdC/CMakeCCompilerId.c @@ -0,0 +1,803 @@ +#ifdef __cplusplus +# error "A C++ compiler has been selected for C." +#endif + +#if defined(__18CXX) +# define ID_VOID_MAIN +#endif +#if defined(__CLASSIC_C__) +/* cv-qualifiers did not exist in K&R C */ +# define const +# define volatile +#endif + +#if !defined(__has_include) +/* If the compiler does not have __has_include, pretend the answer is + always no. */ +# define __has_include(x) 0 +#endif + + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# if defined(__GNUC__) +# define SIMULATE_ID "GNU" +# endif + /* __INTEL_COMPILER = VRP prior to 2021, and then VVVV for 2021 and later, + except that a few beta releases use the old format with V=2021. */ +# if __INTEL_COMPILER < 2021 || __INTEL_COMPILER == 202110 || __INTEL_COMPILER == 202111 +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# if defined(__INTEL_COMPILER_UPDATE) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE) +# else +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# endif +# else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER_UPDATE) + /* The third version component from --version is an update index, + but no macro is provided for it. */ +# define COMPILER_VERSION_PATCH DEC(0) +# endif +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +# elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif (defined(__clang__) && defined(__INTEL_CLANG_COMPILER)) || defined(__INTEL_LLVM_COMPILER) +# define COMPILER_ID "IntelLLVM" +#if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +#endif +#if defined(__GNUC__) +# define SIMULATE_ID "GNU" +#endif +/* __INTEL_LLVM_COMPILER = VVVVRP prior to 2021.2.0, VVVVRRPP for 2021.2.0 and + * later. Look for 6 digit vs. 8 digit version number to decide encoding. + * VVVV is no smaller than the current year when a version is released. + */ +#if __INTEL_LLVM_COMPILER < 1000000L +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 10) +#else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/10000) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 100) +#endif +#if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +#endif +#if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +#elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +#endif +#if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +#endif +#if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +#endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) && __WATCOMC__ < 1200 +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__WATCOMC__) +# define COMPILER_ID "OpenWatcom" + /* __WATCOMC__ = VVRP + 1100 */ +# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__SUNPRO_C) +# define COMPILER_ID "SunPro" +# if __SUNPRO_C >= 0x5100 + /* __SUNPRO_C = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF) +# endif + +#elif defined(__HP_cc) +# define COMPILER_ID "HP" + /* __HP_cc = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100) + +#elif defined(__DECC) +# define COMPILER_ID "Compaq" + /* __DECC_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000) + +#elif defined(__IBMC__) && defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__ibmxl__) && defined(__clang__) +# define COMPILER_ID "XLClang" +# define COMPILER_VERSION_MAJOR DEC(__ibmxl_version__) +# define COMPILER_VERSION_MINOR DEC(__ibmxl_release__) +# define COMPILER_VERSION_PATCH DEC(__ibmxl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__ibmxl_ptf_fix_level__) + + +#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ >= 800 +# define COMPILER_ID "XL" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ < 800 +# define COMPILER_ID "VisualAge" + /* __IBMC__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10) + +#elif defined(__NVCOMPILER) +# define COMPILER_ID "NVHPC" +# define COMPILER_VERSION_MAJOR DEC(__NVCOMPILER_MAJOR__) +# define COMPILER_VERSION_MINOR DEC(__NVCOMPILER_MINOR__) +# if defined(__NVCOMPILER_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__NVCOMPILER_PATCHLEVEL__) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__CLANG_FUJITSU) +# define COMPILER_ID "FujitsuClang" +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# define COMPILER_VERSION_INTERNAL_STR __clang_version__ + + +#elif defined(__FUJITSU) +# define COMPILER_ID "Fujitsu" +# if defined(__FCC_version__) +# define COMPILER_VERSION __FCC_version__ +# elif defined(__FCC_major__) +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# endif +# if defined(__fcc_version) +# define COMPILER_VERSION_INTERNAL DEC(__fcc_version) +# elif defined(__FCC_VERSION) +# define COMPILER_VERSION_INTERNAL DEC(__FCC_VERSION) +# endif + + +#elif defined(__ghs__) +# define COMPILER_ID "GHS" +/* __GHS_VERSION_NUMBER = VVVVRP */ +# ifdef __GHS_VERSION_NUMBER +# define COMPILER_VERSION_MAJOR DEC(__GHS_VERSION_NUMBER / 100) +# define COMPILER_VERSION_MINOR DEC(__GHS_VERSION_NUMBER / 10 % 10) +# define COMPILER_VERSION_PATCH DEC(__GHS_VERSION_NUMBER % 10) +# endif + +#elif defined(__TINYC__) +# define COMPILER_ID "TinyCC" + +#elif defined(__BCC__) +# define COMPILER_ID "Bruce" + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__ARMCC_VERSION) && !defined(__clang__) +# define COMPILER_ID "ARMCC" +#if __ARMCC_VERSION >= 1000000 + /* __ARMCC_VERSION = VRRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#else + /* __ARMCC_VERSION = VRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#endif + + +#elif defined(__clang__) && defined(__apple_build_version__) +# define COMPILER_ID "AppleClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__) + +#elif defined(__clang__) && defined(__ARMCOMPILER_VERSION) +# define COMPILER_ID "ARMClang" + # define COMPILER_VERSION_MAJOR DEC(__ARMCOMPILER_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCOMPILER_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCOMPILER_VERSION % 10000) +# define COMPILER_VERSION_INTERNAL DEC(__ARMCOMPILER_VERSION) + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif + +#elif defined(__GNUC__) +# define COMPILER_ID "GNU" +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# if defined(__GNUC_MINOR__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +#elif defined(__VISUALDSPVERSION__) || defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" +#if defined(__VISUALDSPVERSION__) + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) +#endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" +# if defined(__VER__) && defined(__ICCARM__) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000) +# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000) +# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# elif defined(__VER__) && (defined(__ICCAVR__) || defined(__ICCRX__) || defined(__ICCRH850__) || defined(__ICCRL78__) || defined(__ICC430__) || defined(__ICCRISCV__) || defined(__ICCV850__) || defined(__ICC8051__) || defined(__ICCSTM8__)) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 100) +# define COMPILER_VERSION_MINOR DEC((__VER__) - (((__VER__) / 100)*100)) +# define COMPILER_VERSION_PATCH DEC(__SUBVERSION__) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# endif + +#elif defined(__SDCC_VERSION_MAJOR) || defined(SDCC) +# define COMPILER_ID "SDCC" +# if defined(__SDCC_VERSION_MAJOR) +# define COMPILER_VERSION_MAJOR DEC(__SDCC_VERSION_MAJOR) +# define COMPILER_VERSION_MINOR DEC(__SDCC_VERSION_MINOR) +# define COMPILER_VERSION_PATCH DEC(__SDCC_VERSION_PATCH) +# else + /* SDCC = VRP */ +# define COMPILER_VERSION_MAJOR DEC(SDCC/100) +# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10) +# define COMPILER_VERSION_PATCH DEC(SDCC % 10) +# endif + + +/* These compilers are either not known or too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; +#ifdef SIMULATE_ID +char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]"; +#endif + +#ifdef __QNXNTO__ +char const* qnxnto = "INFO" ":" "qnxnto[]"; +#endif + +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) +char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]"; +#endif + +#define STRINGIFY_HELPER(X) #X +#define STRINGIFY(X) STRINGIFY_HELPER(X) + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__MSYS__) +# define PLATFORM_ID "MSYS" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#elif defined(__WATCOMC__) +# if defined(__LINUX__) +# define PLATFORM_ID "Linux" + +# elif defined(__DOS__) +# define PLATFORM_ID "DOS" + +# elif defined(__OS2__) +# define PLATFORM_ID "OS2" + +# elif defined(__WINDOWS__) +# define PLATFORM_ID "Windows3x" + +# elif defined(__VXWORKS__) +# define PLATFORM_ID "VxWorks" + +# else /* unknown platform */ +# define PLATFORM_ID +# endif + +#elif defined(__INTEGRITY) +# if defined(INT_178B) +# define PLATFORM_ID "Integrity178" + +# else /* regular Integrity */ +# define PLATFORM_ID "Integrity" +# endif + +#else /* unknown platform */ +# define PLATFORM_ID + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_ARM64EC) +# define ARCHITECTURE_ID "ARM64EC" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM64) +# define ARCHITECTURE_ID "ARM64" + +# elif defined(_M_ARM) +# if _M_ARM == 4 +# define ARCHITECTURE_ID "ARMV4I" +# elif _M_ARM == 5 +# define ARCHITECTURE_ID "ARMV5I" +# else +# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM) +# endif + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__WATCOMC__) +# if defined(_M_I86) +# define ARCHITECTURE_ID "I86" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# if defined(__ICCARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__ICCRX__) +# define ARCHITECTURE_ID "RX" + +# elif defined(__ICCRH850__) +# define ARCHITECTURE_ID "RH850" + +# elif defined(__ICCRL78__) +# define ARCHITECTURE_ID "RL78" + +# elif defined(__ICCRISCV__) +# define ARCHITECTURE_ID "RISCV" + +# elif defined(__ICCAVR__) +# define ARCHITECTURE_ID "AVR" + +# elif defined(__ICC430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__ICCV850__) +# define ARCHITECTURE_ID "V850" + +# elif defined(__ICC8051__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__ICCSTM8__) +# define ARCHITECTURE_ID "STM8" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__ghs__) +# if defined(__PPC64__) +# define ARCHITECTURE_ID "PPC64" + +# elif defined(__ppc__) +# define ARCHITECTURE_ID "PPC" + +# elif defined(__ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__x86_64__) +# define ARCHITECTURE_ID "x64" + +# elif defined(__i386__) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__TI_COMPILER_VERSION__) +# if defined(__TI_ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__MSP430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__TMS320C28XX__) +# define ARCHITECTURE_ID "TMS320C28x" + +# elif defined(__TMS320C6X__) || defined(_TMS320C6X) +# define ARCHITECTURE_ID "TMS320C6x" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number. */ +#ifdef COMPILER_VERSION +char const* info_version = "INFO" ":" "compiler_version[" COMPILER_VERSION "]"; + +/* Construct a string literal encoding the version number components. */ +#elif defined(COMPILER_VERSION_MAJOR) +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct a string literal encoding the internal version number. */ +#ifdef COMPILER_VERSION_INTERNAL +char const info_version_internal[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_', + 'i','n','t','e','r','n','a','l','[', + COMPILER_VERSION_INTERNAL,']','\0'}; +#elif defined(COMPILER_VERSION_INTERNAL_STR) +char const* info_version_internal = "INFO" ":" "compiler_version_internal[" COMPILER_VERSION_INTERNAL_STR "]"; +#endif + +/* Construct a string literal encoding the version number components. */ +#ifdef SIMULATE_VERSION_MAJOR +char const info_simulate_version[] = { + 'I', 'N', 'F', 'O', ':', + 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[', + SIMULATE_VERSION_MAJOR, +# ifdef SIMULATE_VERSION_MINOR + '.', SIMULATE_VERSION_MINOR, +# ifdef SIMULATE_VERSION_PATCH + '.', SIMULATE_VERSION_PATCH, +# ifdef SIMULATE_VERSION_TWEAK + '.', SIMULATE_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +#if !defined(__STDC__) && !defined(__clang__) +# if defined(_MSC_VER) || defined(__ibmxl__) || defined(__IBMC__) +# define C_VERSION "90" +# else +# define C_VERSION +# endif +#elif __STDC_VERSION__ > 201710L +# define C_VERSION "23" +#elif __STDC_VERSION__ >= 201710L +# define C_VERSION "17" +#elif __STDC_VERSION__ >= 201000L +# define C_VERSION "11" +#elif __STDC_VERSION__ >= 199901L +# define C_VERSION "99" +#else +# define C_VERSION "90" +#endif +const char* info_language_standard_default = + "INFO" ":" "standard_default[" C_VERSION "]"; + +const char* info_language_extensions_default = "INFO" ":" "extensions_default[" +/* !defined(_MSC_VER) to exclude Clang's MSVC compatibility mode. */ +#if (defined(__clang__) || defined(__GNUC__) || \ + defined(__TI_COMPILER_VERSION__)) && \ + !defined(__STRICT_ANSI__) && !defined(_MSC_VER) + "ON" +#else + "OFF" +#endif +"]"; + +/*--------------------------------------------------------------------------*/ + +#ifdef ID_VOID_MAIN +void main() {} +#else +# if defined(__CLASSIC_C__) +int main(argc, argv) int argc; char *argv[]; +# else +int main(int argc, char* argv[]) +# endif +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; + require += info_arch[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif +#ifdef COMPILER_VERSION_INTERNAL + require += info_version_internal[argc]; +#endif +#ifdef SIMULATE_ID + require += info_simulate[argc]; +#endif +#ifdef SIMULATE_VERSION_MAJOR + require += info_simulate_version[argc]; +#endif +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) + require += info_cray[argc]; +#endif + require += info_language_standard_default[argc]; + require += info_language_extensions_default[argc]; + (void)argv; + return require; +} +#endif diff --git a/build/Project/CMakeFiles/3.22.1/CompilerIdC/a.out b/build/Project/CMakeFiles/3.22.1/CompilerIdC/a.out new file mode 100755 index 000000000..c786756ab Binary files /dev/null and b/build/Project/CMakeFiles/3.22.1/CompilerIdC/a.out differ diff --git a/build/Project/CMakeFiles/3.22.1/CompilerIdCXX/CMakeCXXCompilerId.cpp b/build/Project/CMakeFiles/3.22.1/CompilerIdCXX/CMakeCXXCompilerId.cpp new file mode 100644 index 000000000..25c62a8c3 --- /dev/null +++ b/build/Project/CMakeFiles/3.22.1/CompilerIdCXX/CMakeCXXCompilerId.cpp @@ -0,0 +1,791 @@ +/* This source file must have a .cpp extension so that all C++ compilers + recognize the extension without flags. Borland does not know .cxx for + example. */ +#ifndef __cplusplus +# error "A C compiler has been selected for C++." +#endif + +#if !defined(__has_include) +/* If the compiler does not have __has_include, pretend the answer is + always no. */ +# define __has_include(x) 0 +#endif + + +/* Version number components: V=Version, R=Revision, P=Patch + Version date components: YYYY=Year, MM=Month, DD=Day */ + +#if defined(__COMO__) +# define COMPILER_ID "Comeau" + /* __COMO_VERSION__ = VRR */ +# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100) +# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100) + +#elif defined(__INTEL_COMPILER) || defined(__ICC) +# define COMPILER_ID "Intel" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# if defined(__GNUC__) +# define SIMULATE_ID "GNU" +# endif + /* __INTEL_COMPILER = VRP prior to 2021, and then VVVV for 2021 and later, + except that a few beta releases use the old format with V=2021. */ +# if __INTEL_COMPILER < 2021 || __INTEL_COMPILER == 202110 || __INTEL_COMPILER == 202111 +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10) +# if defined(__INTEL_COMPILER_UPDATE) +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE) +# else +# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10) +# endif +# else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER) +# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER_UPDATE) + /* The third version component from --version is an update index, + but no macro is provided for it. */ +# define COMPILER_VERSION_PATCH DEC(0) +# endif +# if defined(__INTEL_COMPILER_BUILD_DATE) + /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */ +# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE) +# endif +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +# elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif (defined(__clang__) && defined(__INTEL_CLANG_COMPILER)) || defined(__INTEL_LLVM_COMPILER) +# define COMPILER_ID "IntelLLVM" +#if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +#endif +#if defined(__GNUC__) +# define SIMULATE_ID "GNU" +#endif +/* __INTEL_LLVM_COMPILER = VVVVRP prior to 2021.2.0, VVVVRRPP for 2021.2.0 and + * later. Look for 6 digit vs. 8 digit version number to decide encoding. + * VVVV is no smaller than the current year when a version is released. + */ +#if __INTEL_LLVM_COMPILER < 1000000L +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/100) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 10) +#else +# define COMPILER_VERSION_MAJOR DEC(__INTEL_LLVM_COMPILER/10000) +# define COMPILER_VERSION_MINOR DEC(__INTEL_LLVM_COMPILER/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__INTEL_LLVM_COMPILER % 100) +#endif +#if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +#endif +#if defined(__GNUC__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUC__) +#elif defined(__GNUG__) +# define SIMULATE_VERSION_MAJOR DEC(__GNUG__) +#endif +#if defined(__GNUC_MINOR__) +# define SIMULATE_VERSION_MINOR DEC(__GNUC_MINOR__) +#endif +#if defined(__GNUC_PATCHLEVEL__) +# define SIMULATE_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +#endif + +#elif defined(__PATHCC__) +# define COMPILER_ID "PathScale" +# define COMPILER_VERSION_MAJOR DEC(__PATHCC__) +# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__) +# if defined(__PATHCC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__) +# endif + +#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__) +# define COMPILER_ID "Embarcadero" +# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF) +# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF) +# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF) + +#elif defined(__BORLANDC__) +# define COMPILER_ID "Borland" + /* __BORLANDC__ = 0xVRR */ +# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8) +# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF) + +#elif defined(__WATCOMC__) && __WATCOMC__ < 1200 +# define COMPILER_ID "Watcom" + /* __WATCOMC__ = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__WATCOMC__) +# define COMPILER_ID "OpenWatcom" + /* __WATCOMC__ = VVRP + 1100 */ +# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100) +# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10) +# if (__WATCOMC__ % 10) > 0 +# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10) +# endif + +#elif defined(__SUNPRO_CC) +# define COMPILER_ID "SunPro" +# if __SUNPRO_CC >= 0x5100 + /* __SUNPRO_CC = 0xVRRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# else + /* __SUNPRO_CC = 0xVRP */ +# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8) +# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF) +# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF) +# endif + +#elif defined(__HP_aCC) +# define COMPILER_ID "HP" + /* __HP_aCC = VVRRPP */ +# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000) +# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100) +# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100) + +#elif defined(__DECCXX) +# define COMPILER_ID "Compaq" + /* __DECCXX_VER = VVRRTPPPP */ +# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000) +# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100) +# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000) + +#elif defined(__IBMCPP__) && defined(__COMPILER_VER__) +# define COMPILER_ID "zOS" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__ibmxl__) && defined(__clang__) +# define COMPILER_ID "XLClang" +# define COMPILER_VERSION_MAJOR DEC(__ibmxl_version__) +# define COMPILER_VERSION_MINOR DEC(__ibmxl_release__) +# define COMPILER_VERSION_PATCH DEC(__ibmxl_modification__) +# define COMPILER_VERSION_TWEAK DEC(__ibmxl_ptf_fix_level__) + + +#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ >= 800 +# define COMPILER_ID "XL" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ < 800 +# define COMPILER_ID "VisualAge" + /* __IBMCPP__ = VRP */ +# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100) +# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10) +# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10) + +#elif defined(__NVCOMPILER) +# define COMPILER_ID "NVHPC" +# define COMPILER_VERSION_MAJOR DEC(__NVCOMPILER_MAJOR__) +# define COMPILER_VERSION_MINOR DEC(__NVCOMPILER_MINOR__) +# if defined(__NVCOMPILER_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__NVCOMPILER_PATCHLEVEL__) +# endif + +#elif defined(__PGI) +# define COMPILER_ID "PGI" +# define COMPILER_VERSION_MAJOR DEC(__PGIC__) +# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__) +# if defined(__PGIC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__) +# endif + +#elif defined(_CRAYC) +# define COMPILER_ID "Cray" +# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR) +# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR) + +#elif defined(__TI_COMPILER_VERSION__) +# define COMPILER_ID "TI" + /* __TI_COMPILER_VERSION__ = VVVRRRPPP */ +# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000) +# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000) +# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000) + +#elif defined(__CLANG_FUJITSU) +# define COMPILER_ID "FujitsuClang" +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# define COMPILER_VERSION_INTERNAL_STR __clang_version__ + + +#elif defined(__FUJITSU) +# define COMPILER_ID "Fujitsu" +# if defined(__FCC_version__) +# define COMPILER_VERSION __FCC_version__ +# elif defined(__FCC_major__) +# define COMPILER_VERSION_MAJOR DEC(__FCC_major__) +# define COMPILER_VERSION_MINOR DEC(__FCC_minor__) +# define COMPILER_VERSION_PATCH DEC(__FCC_patchlevel__) +# endif +# if defined(__fcc_version) +# define COMPILER_VERSION_INTERNAL DEC(__fcc_version) +# elif defined(__FCC_VERSION) +# define COMPILER_VERSION_INTERNAL DEC(__FCC_VERSION) +# endif + + +#elif defined(__ghs__) +# define COMPILER_ID "GHS" +/* __GHS_VERSION_NUMBER = VVVVRP */ +# ifdef __GHS_VERSION_NUMBER +# define COMPILER_VERSION_MAJOR DEC(__GHS_VERSION_NUMBER / 100) +# define COMPILER_VERSION_MINOR DEC(__GHS_VERSION_NUMBER / 10 % 10) +# define COMPILER_VERSION_PATCH DEC(__GHS_VERSION_NUMBER % 10) +# endif + +#elif defined(__SCO_VERSION__) +# define COMPILER_ID "SCO" + +#elif defined(__ARMCC_VERSION) && !defined(__clang__) +# define COMPILER_ID "ARMCC" +#if __ARMCC_VERSION >= 1000000 + /* __ARMCC_VERSION = VRRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#else + /* __ARMCC_VERSION = VRPPPP */ + # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000) + # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10) + # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000) +#endif + + +#elif defined(__clang__) && defined(__apple_build_version__) +# define COMPILER_ID "AppleClang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif +# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__) + +#elif defined(__clang__) && defined(__ARMCOMPILER_VERSION) +# define COMPILER_ID "ARMClang" + # define COMPILER_VERSION_MAJOR DEC(__ARMCOMPILER_VERSION/1000000) + # define COMPILER_VERSION_MINOR DEC(__ARMCOMPILER_VERSION/10000 % 100) + # define COMPILER_VERSION_PATCH DEC(__ARMCOMPILER_VERSION % 10000) +# define COMPILER_VERSION_INTERNAL DEC(__ARMCOMPILER_VERSION) + +#elif defined(__clang__) +# define COMPILER_ID "Clang" +# if defined(_MSC_VER) +# define SIMULATE_ID "MSVC" +# endif +# define COMPILER_VERSION_MAJOR DEC(__clang_major__) +# define COMPILER_VERSION_MINOR DEC(__clang_minor__) +# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__) +# if defined(_MSC_VER) + /* _MSC_VER = VVRR */ +# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100) +# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100) +# endif + +#elif defined(__GNUC__) || defined(__GNUG__) +# define COMPILER_ID "GNU" +# if defined(__GNUC__) +# define COMPILER_VERSION_MAJOR DEC(__GNUC__) +# else +# define COMPILER_VERSION_MAJOR DEC(__GNUG__) +# endif +# if defined(__GNUC_MINOR__) +# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__) +# endif +# if defined(__GNUC_PATCHLEVEL__) +# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__) +# endif + +#elif defined(_MSC_VER) +# define COMPILER_ID "MSVC" + /* _MSC_VER = VVRR */ +# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100) +# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100) +# if defined(_MSC_FULL_VER) +# if _MSC_VER >= 1400 + /* _MSC_FULL_VER = VVRRPPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000) +# else + /* _MSC_FULL_VER = VVRRPPPP */ +# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000) +# endif +# endif +# if defined(_MSC_BUILD) +# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD) +# endif + +#elif defined(__VISUALDSPVERSION__) || defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__) +# define COMPILER_ID "ADSP" +#if defined(__VISUALDSPVERSION__) + /* __VISUALDSPVERSION__ = 0xVVRRPP00 */ +# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24) +# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF) +# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF) +#endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# define COMPILER_ID "IAR" +# if defined(__VER__) && defined(__ICCARM__) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000) +# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000) +# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# elif defined(__VER__) && (defined(__ICCAVR__) || defined(__ICCRX__) || defined(__ICCRH850__) || defined(__ICCRL78__) || defined(__ICC430__) || defined(__ICCRISCV__) || defined(__ICCV850__) || defined(__ICC8051__) || defined(__ICCSTM8__)) +# define COMPILER_VERSION_MAJOR DEC((__VER__) / 100) +# define COMPILER_VERSION_MINOR DEC((__VER__) - (((__VER__) / 100)*100)) +# define COMPILER_VERSION_PATCH DEC(__SUBVERSION__) +# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__) +# endif + + +/* These compilers are either not known or too old to define an + identification macro. Try to identify the platform and guess that + it is the native compiler. */ +#elif defined(__hpux) || defined(__hpua) +# define COMPILER_ID "HP" + +#else /* unknown compiler */ +# define COMPILER_ID "" +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]"; +#ifdef SIMULATE_ID +char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]"; +#endif + +#ifdef __QNXNTO__ +char const* qnxnto = "INFO" ":" "qnxnto[]"; +#endif + +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) +char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]"; +#endif + +#define STRINGIFY_HELPER(X) #X +#define STRINGIFY(X) STRINGIFY_HELPER(X) + +/* Identify known platforms by name. */ +#if defined(__linux) || defined(__linux__) || defined(linux) +# define PLATFORM_ID "Linux" + +#elif defined(__MSYS__) +# define PLATFORM_ID "MSYS" + +#elif defined(__CYGWIN__) +# define PLATFORM_ID "Cygwin" + +#elif defined(__MINGW32__) +# define PLATFORM_ID "MinGW" + +#elif defined(__APPLE__) +# define PLATFORM_ID "Darwin" + +#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32) +# define PLATFORM_ID "Windows" + +#elif defined(__FreeBSD__) || defined(__FreeBSD) +# define PLATFORM_ID "FreeBSD" + +#elif defined(__NetBSD__) || defined(__NetBSD) +# define PLATFORM_ID "NetBSD" + +#elif defined(__OpenBSD__) || defined(__OPENBSD) +# define PLATFORM_ID "OpenBSD" + +#elif defined(__sun) || defined(sun) +# define PLATFORM_ID "SunOS" + +#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__) +# define PLATFORM_ID "AIX" + +#elif defined(__hpux) || defined(__hpux__) +# define PLATFORM_ID "HP-UX" + +#elif defined(__HAIKU__) +# define PLATFORM_ID "Haiku" + +#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS) +# define PLATFORM_ID "BeOS" + +#elif defined(__QNX__) || defined(__QNXNTO__) +# define PLATFORM_ID "QNX" + +#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__) +# define PLATFORM_ID "Tru64" + +#elif defined(__riscos) || defined(__riscos__) +# define PLATFORM_ID "RISCos" + +#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__) +# define PLATFORM_ID "SINIX" + +#elif defined(__UNIX_SV__) +# define PLATFORM_ID "UNIX_SV" + +#elif defined(__bsdos__) +# define PLATFORM_ID "BSDOS" + +#elif defined(_MPRAS) || defined(MPRAS) +# define PLATFORM_ID "MP-RAS" + +#elif defined(__osf) || defined(__osf__) +# define PLATFORM_ID "OSF1" + +#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv) +# define PLATFORM_ID "SCO_SV" + +#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX) +# define PLATFORM_ID "ULTRIX" + +#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX) +# define PLATFORM_ID "Xenix" + +#elif defined(__WATCOMC__) +# if defined(__LINUX__) +# define PLATFORM_ID "Linux" + +# elif defined(__DOS__) +# define PLATFORM_ID "DOS" + +# elif defined(__OS2__) +# define PLATFORM_ID "OS2" + +# elif defined(__WINDOWS__) +# define PLATFORM_ID "Windows3x" + +# elif defined(__VXWORKS__) +# define PLATFORM_ID "VxWorks" + +# else /* unknown platform */ +# define PLATFORM_ID +# endif + +#elif defined(__INTEGRITY) +# if defined(INT_178B) +# define PLATFORM_ID "Integrity178" + +# else /* regular Integrity */ +# define PLATFORM_ID "Integrity" +# endif + +#else /* unknown platform */ +# define PLATFORM_ID + +#endif + +/* For windows compilers MSVC and Intel we can determine + the architecture of the compiler being used. This is because + the compilers do not have flags that can change the architecture, + but rather depend on which compiler is being used +*/ +#if defined(_WIN32) && defined(_MSC_VER) +# if defined(_M_IA64) +# define ARCHITECTURE_ID "IA64" + +# elif defined(_M_ARM64EC) +# define ARCHITECTURE_ID "ARM64EC" + +# elif defined(_M_X64) || defined(_M_AMD64) +# define ARCHITECTURE_ID "x64" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# elif defined(_M_ARM64) +# define ARCHITECTURE_ID "ARM64" + +# elif defined(_M_ARM) +# if _M_ARM == 4 +# define ARCHITECTURE_ID "ARMV4I" +# elif _M_ARM == 5 +# define ARCHITECTURE_ID "ARMV5I" +# else +# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM) +# endif + +# elif defined(_M_MIPS) +# define ARCHITECTURE_ID "MIPS" + +# elif defined(_M_SH) +# define ARCHITECTURE_ID "SHx" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__WATCOMC__) +# if defined(_M_I86) +# define ARCHITECTURE_ID "I86" + +# elif defined(_M_IX86) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC) +# if defined(__ICCARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__ICCRX__) +# define ARCHITECTURE_ID "RX" + +# elif defined(__ICCRH850__) +# define ARCHITECTURE_ID "RH850" + +# elif defined(__ICCRL78__) +# define ARCHITECTURE_ID "RL78" + +# elif defined(__ICCRISCV__) +# define ARCHITECTURE_ID "RISCV" + +# elif defined(__ICCAVR__) +# define ARCHITECTURE_ID "AVR" + +# elif defined(__ICC430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__ICCV850__) +# define ARCHITECTURE_ID "V850" + +# elif defined(__ICC8051__) +# define ARCHITECTURE_ID "8051" + +# elif defined(__ICCSTM8__) +# define ARCHITECTURE_ID "STM8" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__ghs__) +# if defined(__PPC64__) +# define ARCHITECTURE_ID "PPC64" + +# elif defined(__ppc__) +# define ARCHITECTURE_ID "PPC" + +# elif defined(__ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__x86_64__) +# define ARCHITECTURE_ID "x64" + +# elif defined(__i386__) +# define ARCHITECTURE_ID "X86" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#elif defined(__TI_COMPILER_VERSION__) +# if defined(__TI_ARM__) +# define ARCHITECTURE_ID "ARM" + +# elif defined(__MSP430__) +# define ARCHITECTURE_ID "MSP430" + +# elif defined(__TMS320C28XX__) +# define ARCHITECTURE_ID "TMS320C28x" + +# elif defined(__TMS320C6X__) || defined(_TMS320C6X) +# define ARCHITECTURE_ID "TMS320C6x" + +# else /* unknown architecture */ +# define ARCHITECTURE_ID "" +# endif + +#else +# define ARCHITECTURE_ID +#endif + +/* Convert integer to decimal digit literals. */ +#define DEC(n) \ + ('0' + (((n) / 10000000)%10)), \ + ('0' + (((n) / 1000000)%10)), \ + ('0' + (((n) / 100000)%10)), \ + ('0' + (((n) / 10000)%10)), \ + ('0' + (((n) / 1000)%10)), \ + ('0' + (((n) / 100)%10)), \ + ('0' + (((n) / 10)%10)), \ + ('0' + ((n) % 10)) + +/* Convert integer to hex digit literals. */ +#define HEX(n) \ + ('0' + ((n)>>28 & 0xF)), \ + ('0' + ((n)>>24 & 0xF)), \ + ('0' + ((n)>>20 & 0xF)), \ + ('0' + ((n)>>16 & 0xF)), \ + ('0' + ((n)>>12 & 0xF)), \ + ('0' + ((n)>>8 & 0xF)), \ + ('0' + ((n)>>4 & 0xF)), \ + ('0' + ((n) & 0xF)) + +/* Construct a string literal encoding the version number. */ +#ifdef COMPILER_VERSION +char const* info_version = "INFO" ":" "compiler_version[" COMPILER_VERSION "]"; + +/* Construct a string literal encoding the version number components. */ +#elif defined(COMPILER_VERSION_MAJOR) +char const info_version[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[', + COMPILER_VERSION_MAJOR, +# ifdef COMPILER_VERSION_MINOR + '.', COMPILER_VERSION_MINOR, +# ifdef COMPILER_VERSION_PATCH + '.', COMPILER_VERSION_PATCH, +# ifdef COMPILER_VERSION_TWEAK + '.', COMPILER_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct a string literal encoding the internal version number. */ +#ifdef COMPILER_VERSION_INTERNAL +char const info_version_internal[] = { + 'I', 'N', 'F', 'O', ':', + 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_', + 'i','n','t','e','r','n','a','l','[', + COMPILER_VERSION_INTERNAL,']','\0'}; +#elif defined(COMPILER_VERSION_INTERNAL_STR) +char const* info_version_internal = "INFO" ":" "compiler_version_internal[" COMPILER_VERSION_INTERNAL_STR "]"; +#endif + +/* Construct a string literal encoding the version number components. */ +#ifdef SIMULATE_VERSION_MAJOR +char const info_simulate_version[] = { + 'I', 'N', 'F', 'O', ':', + 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[', + SIMULATE_VERSION_MAJOR, +# ifdef SIMULATE_VERSION_MINOR + '.', SIMULATE_VERSION_MINOR, +# ifdef SIMULATE_VERSION_PATCH + '.', SIMULATE_VERSION_PATCH, +# ifdef SIMULATE_VERSION_TWEAK + '.', SIMULATE_VERSION_TWEAK, +# endif +# endif +# endif + ']','\0'}; +#endif + +/* Construct the string literal in pieces to prevent the source from + getting matched. Store it in a pointer rather than an array + because some compilers will just produce instructions to fill the + array rather than assigning a pointer to a static array. */ +char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]"; +char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]"; + + + +#if defined(__INTEL_COMPILER) && defined(_MSVC_LANG) && _MSVC_LANG < 201403L +# if defined(__INTEL_CXX11_MODE__) +# if defined(__cpp_aggregate_nsdmi) +# define CXX_STD 201402L +# else +# define CXX_STD 201103L +# endif +# else +# define CXX_STD 199711L +# endif +#elif defined(_MSC_VER) && defined(_MSVC_LANG) +# define CXX_STD _MSVC_LANG +#else +# define CXX_STD __cplusplus +#endif + +const char* info_language_standard_default = "INFO" ":" "standard_default[" +#if CXX_STD > 202002L + "23" +#elif CXX_STD > 201703L + "20" +#elif CXX_STD >= 201703L + "17" +#elif CXX_STD >= 201402L + "14" +#elif CXX_STD >= 201103L + "11" +#else + "98" +#endif +"]"; + +const char* info_language_extensions_default = "INFO" ":" "extensions_default[" +/* !defined(_MSC_VER) to exclude Clang's MSVC compatibility mode. */ +#if (defined(__clang__) || defined(__GNUC__) || \ + defined(__TI_COMPILER_VERSION__)) && \ + !defined(__STRICT_ANSI__) && !defined(_MSC_VER) + "ON" +#else + "OFF" +#endif +"]"; + +/*--------------------------------------------------------------------------*/ + +int main(int argc, char* argv[]) +{ + int require = 0; + require += info_compiler[argc]; + require += info_platform[argc]; +#ifdef COMPILER_VERSION_MAJOR + require += info_version[argc]; +#endif +#ifdef COMPILER_VERSION_INTERNAL + require += info_version_internal[argc]; +#endif +#ifdef SIMULATE_ID + require += info_simulate[argc]; +#endif +#ifdef SIMULATE_VERSION_MAJOR + require += info_simulate_version[argc]; +#endif +#if defined(__CRAYXT_COMPUTE_LINUX_TARGET) + require += info_cray[argc]; +#endif + require += info_language_standard_default[argc]; + require += info_language_extensions_default[argc]; + (void)argv; + return require; +} diff --git a/build/Project/CMakeFiles/3.22.1/CompilerIdCXX/a.out b/build/Project/CMakeFiles/3.22.1/CompilerIdCXX/a.out new file mode 100755 index 000000000..9944be481 Binary files /dev/null and b/build/Project/CMakeFiles/3.22.1/CompilerIdCXX/a.out differ diff --git a/build/Project/CMakeFiles/cmake.check_cache b/build/Project/CMakeFiles/cmake.check_cache new file mode 100644 index 000000000..3dccd7317 --- /dev/null +++ b/build/Project/CMakeFiles/cmake.check_cache @@ -0,0 +1 @@ +# This file is generated by cmake for dependency checking of the CMakeCache.txt file diff --git a/build/Project/cmake_args.last b/build/Project/cmake_args.last new file mode 100644 index 000000000..4af18322e --- /dev/null +++ b/build/Project/cmake_args.last @@ -0,0 +1 @@ +None \ No newline at end of file diff --git a/build/Project/colcon_build.rc b/build/Project/colcon_build.rc new file mode 100644 index 000000000..d00491fd7 --- /dev/null +++ b/build/Project/colcon_build.rc @@ -0,0 +1 @@ +1 diff --git a/build/Project/colcon_command_prefix_build.sh b/build/Project/colcon_command_prefix_build.sh new file mode 100644 index 000000000..f9867d513 --- /dev/null +++ b/build/Project/colcon_command_prefix_build.sh @@ -0,0 +1 @@ +# generated from colcon_core/shell/template/command_prefix.sh.em diff --git a/build/Project/colcon_command_prefix_build.sh.env b/build/Project/colcon_command_prefix_build.sh.env new file mode 100644 index 000000000..e8ebca143 --- /dev/null +++ b/build/Project/colcon_command_prefix_build.sh.env @@ -0,0 +1,58 @@ +AMENT_PREFIX_PATH=/home/nazmus/ros2_ws/install/soccer_msgs:/home/nazmus/ros2_ws/install/my_robot_controller:/opt/ros/humble +CMAKE_PREFIX_PATH=/home/nazmus/ros2_ws/install/soccer_msgs +COLCON=1 +COLCON_PREFIX_PATH=/home/nazmus/ros2_ws/install +COLORTERM=truecolor +DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/1000/bus +DESKTOP_SESSION=ubuntu +DISPLAY=:1 +GDMSESSION=ubuntu +GNOME_DESKTOP_SESSION_ID=this-is-deprecated +GNOME_SHELL_SESSION_MODE=ubuntu +GNOME_TERMINAL_SCREEN=/org/gnome/Terminal/screen/33f68e31_a999_42af_b75e_dd4f5cce86ed +GNOME_TERMINAL_SERVICE=:1.108 +GPG_AGENT_INFO=/run/user/1000/gnupg/S.gpg-agent:0:1 +GTK_IM_MODULE=ibus +GTK_MODULES=gail:atk-bridge +HOME=/home/nazmus +LANG=en_CA.UTF-8 +LANGUAGE=en_CA:en +LD_LIBRARY_PATH=/home/nazmus/ros2_ws/install/soccer_msgs/lib:/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib +LESSCLOSE=/usr/bin/lesspipe %s %s +LESSOPEN=| /usr/bin/lesspipe %s +LOGNAME=nazmus +LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36: +OLDPWD=/home/nazmus/ros2_ws +PATH=/opt/ros/humble/bin:/home/nazmus/.local/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/snap/bin:/home/nazmus/.local/share/JetBrains/Toolbox/scripts +PWD=/home/nazmus/ros2_ws/src/soccerbot/build/Project +PYTHONPATH=/home/nazmus/ros2_ws/install/soccer_msgs/local/lib/python3.10/dist-packages:/home/nazmus/ros2_ws/build/my_robot_controller:/home/nazmus/ros2_ws/install/my_robot_controller/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages +QT_ACCESSIBILITY=1 +QT_IM_MODULE=ibus +ROS_DISTRO=humble +ROS_LOCALHOST_ONLY=0 +ROS_PYTHON_VERSION=3 +ROS_VERSION=2 +SESSION_MANAGER=local/nazmus-Victus-by-HP-Gaming-Laptop-15-fb1xxx:@/tmp/.ICE-unix/2074,unix/nazmus-Victus-by-HP-Gaming-Laptop-15-fb1xxx:/tmp/.ICE-unix/2074 +SHELL=/bin/bash +SHLVL=3 +SSH_AGENT_LAUNCHER=gnome-keyring +SSH_AUTH_SOCK=/run/user/1000/keyring/ssh +SYSTEMD_EXEC_PID=2097 +TERM=xterm-256color +TERMINAL_EMULATOR=JetBrains-JediTerm +TERM_SESSION_ID=5e88e610-3e30-4ef9-8b58-99522d671354 +USER=nazmus +USERNAME=nazmus +VTE_VERSION=6800 +WINDOWPATH=2 +XAUTHORITY=/run/user/1000/gdm/Xauthority +XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg +XDG_CURRENT_DESKTOP=ubuntu:GNOME +XDG_DATA_DIRS=/usr/share/ubuntu:/usr/share/gnome:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop +XDG_MENU_PREFIX=gnome- +XDG_RUNTIME_DIR=/run/user/1000 +XDG_SESSION_CLASS=user +XDG_SESSION_DESKTOP=ubuntu +XDG_SESSION_TYPE=x11 +XMODIFIERS=@im=ibus +_=/usr/bin/colcon diff --git a/compose.yaml b/compose.yaml index fd675b99a..9e0743024 100644 --- a/compose.yaml +++ b/compose.yaml @@ -22,7 +22,7 @@ services: file: tools/docker/compose.autonomy.yaml service: soccerbot_no_cuda environment: - PYTHONPATH: $$PYTHONPATH:/home/robosoccer/.local/lib/python3.8/site-packages:/opt/ros/noetic/lib/python3/dist-packages:/home/robosoccer/catkin_ws/devel/lib/python3/dist-packages + PYTHONPATH: $$PYTHONPATH:/home/robosoccer/.local/lib/python3.8/site-packages:/opt/ros/noetic/lib/python3/dist-packages:/home/robosoccer/ros2_ws/devel/lib/python3/dist-packages soccerbot: <<: *gpu diff --git a/external/webots b/external/webots new file mode 160000 index 000000000..a5ad48b34 --- /dev/null +++ b/external/webots @@ -0,0 +1 @@ +Subproject commit a5ad48b34b281bc26042c843229514a8fd37b076 diff --git a/external/yolov5 b/external/yolov5 new file mode 160000 index 000000000..eef90572b --- /dev/null +++ b/external/yolov5 @@ -0,0 +1 @@ +Subproject commit eef90572bf11602b17816a1721980cdb07a95eb2 diff --git a/install/.colcon_install_layout b/install/.colcon_install_layout new file mode 100644 index 000000000..3aad5336a --- /dev/null +++ b/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/install/COLCON_IGNORE b/install/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/install/_local_setup_util_ps1.py b/install/_local_setup_util_ps1.py new file mode 100644 index 000000000..3c6d9e877 --- /dev/null +++ b/install/_local_setup_util_ps1.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/install/_local_setup_util_sh.py b/install/_local_setup_util_sh.py new file mode 100644 index 000000000..f67eaa989 --- /dev/null +++ b/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/install/local_setup.bash b/install/local_setup.bash new file mode 100644 index 000000000..03f00256c --- /dev/null +++ b/install/local_setup.bash @@ -0,0 +1,121 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.ps1 b/install/local_setup.ps1 new file mode 100644 index 000000000..6f68c8ded --- /dev/null +++ b/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/install/local_setup.sh b/install/local_setup.sh new file mode 100644 index 000000000..88f77cf6e --- /dev/null +++ b/install/local_setup.sh @@ -0,0 +1,137 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/nazmus/ros2_ws/src/soccerbot/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "_colcon_prefix_sh_source_script() { + if [ -f \"\$1\" ]; then + if [ -n \"\$COLCON_TRACE\" ]; then + echo \"# . \\\"\$1\\\"\" + fi + . \"\$1\" + else + echo \"not found: \\\"\$1\\\"\" 1>&2 + fi + }" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/install/local_setup.zsh b/install/local_setup.zsh new file mode 100644 index 000000000..b6487102f --- /dev/null +++ b/install/local_setup.zsh @@ -0,0 +1,134 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/install/setup.bash b/install/setup.bash new file mode 100644 index 000000000..547b15e16 --- /dev/null +++ b/install/setup.bash @@ -0,0 +1,34 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/nazmus/ros2_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/install/setup.ps1 b/install/setup.ps1 new file mode 100644 index 000000000..3799a353c --- /dev/null +++ b/install/setup.ps1 @@ -0,0 +1,30 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/nazmus/ros2_ws/install\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/install/setup.sh b/install/setup.sh new file mode 100644 index 000000000..ffd99f42e --- /dev/null +++ b/install/setup.sh @@ -0,0 +1,49 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/nazmus/ros2_ws/src/soccerbot/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/nazmus/ros2_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/install/setup.zsh b/install/setup.zsh new file mode 100644 index 000000000..c07f8ef87 --- /dev/null +++ b/install/setup.zsh @@ -0,0 +1,34 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/humble" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/nazmus/ros2_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/log/COLCON_IGNORE b/log/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/log/latest b/log/latest new file mode 120000 index 000000000..b57d247c7 --- /dev/null +++ b/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/log/latest_build b/log/latest_build new file mode 120000 index 000000000..9f0a3bd1b --- /dev/null +++ b/log/latest_build @@ -0,0 +1 @@ +build_2025-06-04_13-49-41 \ No newline at end of file diff --git a/log/latest_list b/log/latest_list new file mode 120000 index 000000000..2bb9e2dc6 --- /dev/null +++ b/log/latest_list @@ -0,0 +1 @@ +list_2025-06-04_13-35-36 \ No newline at end of file diff --git a/pyproject.toml b/pyproject.toml index c2e637291..f0e31e4ec 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -46,7 +46,7 @@ omit =[ "*/usr/local/lib/*", "*/usr/lib/*", "*/opt/ros/noetic/lib/python3/*", - "*/home/$USER/catkin_ws/devel/lib/python3/dist-packages/*", + "*/home/$USER/ros2_ws/devel/lib/python3/dist-packages/*", "*/home/$USER/.local/lib/python3.8/site-packages/*", "*/__init__.py", "*/setup.py", @@ -64,7 +64,7 @@ omit =[ "*/usr/local/lib/*", "*/usr/lib/*", "*/opt/ros/noetic/lib/python3/*", - "*/home/$USER/catkin_ws/devel/lib/python3/dist-packages/*", + "*/home/$USER/ros2_ws/devel/lib/python3/dist-packages/*", "*/home/$USER/.local/lib/python3.8/site-packages/*", "*/__init__.py", "*/setup.py", diff --git a/soccer_common/CMakeLists.txt b/soccer_common/CMakeLists.txt index 4c9374420..f73c9ebd1 100644 --- a/soccer_common/CMakeLists.txt +++ b/soccer_common/CMakeLists.txt @@ -8,15 +8,15 @@ set(PKG_DEPS ) find_package( - catkin + ros2 REQUIRED ${PKG_DEPS} ) -catkin_package(CATKIN_DEPENDS soccer_msgs ${PKG_DEPS}) +ros2_package(CATKIN_DEPENDS soccer_msgs ${PKG_DEPS}) # ############################################################################## # Build ## # ############################################################################## -include_directories(${catkin_INCLUDE_DIRS}) +include_directories(${ros2_INCLUDE_DIRS}) -catkin_python_setup() +ros2_python_setup() diff --git a/soccer_common/package.xml b/soccer_common/package.xml index db7fa294d..5f8dd895a 100644 --- a/soccer_common/package.xml +++ b/soccer_common/package.xml @@ -6,7 +6,7 @@ vuwij@me.com BSD - catkin + ros2 rospy geometry_msgs soccer_msgs diff --git a/soccer_common/setup.py b/soccer_common/setup.py index e20781f23..2b0ce47f2 100644 --- a/soccer_common/setup.py +++ b/soccer_common/setup.py @@ -1,6 +1,6 @@ from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from ros2_pkg.python_setup import generate_distutils_setup # fmt: off d = generate_distutils_setup( diff --git a/soccer_control/soccer_pycontrol/CMakeLists.txt b/soccer_control/soccer_pycontrol/CMakeLists.txt index d6da60793..959a3cf2f 100644 --- a/soccer_control/soccer_pycontrol/CMakeLists.txt +++ b/soccer_control/soccer_pycontrol/CMakeLists.txt @@ -2,12 +2,12 @@ cmake_minimum_required(VERSION 3.0.2) project(soccer_pycontrol) find_package( - catkin + ros2 REQUIRED COMPONENTS rospy soccer_common ) -catkin_package() -catkin_python_setup() +ros2_package() +ros2_python_setup() -include_directories(${catkin_INCLUDE_DIRS}) +include_directories(${ros2_INCLUDE_DIRS}) diff --git a/soccer_control/soccer_pycontrol/config/assembly/assembly.yaml b/soccer_control/soccer_pycontrol/config/assembly/assembly.yaml index 85fa05a07..c36c5bced 100644 --- a/soccer_control/soccer_pycontrol/config/assembly/assembly.yaml +++ b/soccer_control/soccer_pycontrol/config/assembly/assembly.yaml @@ -3,19 +3,19 @@ robot_model: bez2 # KINEMATIC DATA #: Height of the robot's torso (center between two arms) while walking -walking_torso_height: 0.37 -arm_0_center: -0.45 +walking_torso_height: 0.43 +arm_0_center: 2.512 arm_1_center: 2.512 # merge_fixed_links: true # STABILIZE -walking_pitch_kp: 0 #2.3 1 -walking_pitch_kd: 0 #1 -walking_pitch_ki: 0.000 +walking_pitch_kp: -0.25 #0.275 +walking_pitch_kd: 0.0 #1 +walking_pitch_ki: -0.00001 walking_pitch_setpoint: -0.0 walking_pitch_offset: 0.0 -walking_roll_kp: 0 #1.5 1 +walking_roll_kp: 0.3 #0.5 walking_roll_kd: 0 #0.5 walking_roll_ki: 0.0 walking_roll_setpoint: -0.0 @@ -24,23 +24,23 @@ walking_roll_offset: 0.0 ### WALK ENGINE # Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency # Timing parameters -control_frequency: 0.01 -single_support_duration: 0.3 # Duration of single support phase [s] +control_frequency: 0.005 +single_support_duration: 0.2 # Duration of single support phase [s] single_support_timesteps: 10 # Number of planning timesteps per single support phase -double_support_ratio: 0.0 # Ratio of double support (0.0 to 1.0) +double_support_ratio: 0.1 # Ratio of double support (0.0 to 1.0) startend_double_support_ratio: 2.0 # Ratio duration of supports for starting and stopping walk planned_timesteps: 48 # Number of timesteps planned ahead replan_timesteps: 10 # Replanning each n timesteps # Posture parameters -walk_com_height: 0.27 # Constant height for the CoM [m] +walk_com_height: 0.24 # Constant height for the CoM [m] walk_foot_height: 0.04 # Height of foot rising while walking [m] -walk_trunk_pitch: 0.0 # Trunk pitch angle [rad] +walk_trunk_pitch: 0.15 # Trunk pitch angle [rad] walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0) # Feet parameters -foot_length: 0.1576 # Foot length [m] -foot_width: 0.092 # Foot width [m] +foot_length: 0.1200 # Foot length [m] +foot_width: 0.072 # Foot width [m] feet_spacing: 0.122 # Lateral feet spacing [m] zmp_margin: 0.02 # ZMP margin [m] foot_zmp_target_x: 0.0 # Reference target ZMP position in the foot [m] @@ -48,6 +48,6 @@ foot_zmp_target_y: 0.0 # Reference target ZMP position in the foot [m] # Limit parameters walk_max_dtheta: 1 # Maximum dtheta per step [rad] -walk_max_dy: 0.04 # Maximum dy per step [m] +walk_max_dy: 0.08 # Maximum dy per step [m] walk_max_dx_forward: 0.08 # Maximum dx per step forward [m] walk_max_dx_backward: 0.03 # Maximum dx per step backward [m] diff --git a/soccer_control/soccer_pycontrol/config/assembly/assembly_sim.yaml b/soccer_control/soccer_pycontrol/config/assembly/assembly_sim.yaml index e6e9032e0..6196bb6c3 100644 --- a/soccer_control/soccer_pycontrol/config/assembly/assembly_sim.yaml +++ b/soccer_control/soccer_pycontrol/config/assembly/assembly_sim.yaml @@ -3,7 +3,7 @@ robot_model: bez2 # KINEMATIC DATA #: Height of the robot's torso (center between two arms) while walking -walking_torso_height: 0.37 +walking_torso_height: 0.43 arm_0_center: -0.45 arm_1_center: 2.512 # merge_fixed_links: true @@ -33,7 +33,7 @@ planned_timesteps: 48 # Number of timesteps planned ahead replan_timesteps: 10 # Replanning each n timesteps # Posture parameters -walk_com_height: 0.27 # Constant height for the CoM [m] +walk_com_height: 0.23 # Constant height for the CoM [m] walk_foot_height: 0.04 # Height of foot rising while walking [m] walk_trunk_pitch: 0.0 # Trunk pitch angle [rad] walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0) @@ -42,7 +42,7 @@ walk_foot_rise_ratio: 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0) foot_length: 0.1576 # Foot length [m] foot_width: 0.092 # Foot width [m] feet_spacing: 0.122 # Lateral feet spacing [m] -zmp_margin: 0.02 # ZMP margin [m] +zmp_margin: 0.01 # ZMP margin [m] foot_zmp_target_x: 0.0 # Reference target ZMP position in the foot [m] foot_zmp_target_y: 0.0 # Reference target ZMP position in the foot [m] diff --git a/soccer_control/soccer_pycontrol/package.xml b/soccer_control/soccer_pycontrol/package.xml index fe191d942..e7c605b2a 100644 --- a/soccer_control/soccer_pycontrol/package.xml +++ b/soccer_control/soccer_pycontrol/package.xml @@ -6,7 +6,7 @@ Nam BSD - catkin + ros2 rospy soccer_common diff --git a/soccer_control/soccer_pycontrol/setup.py b/soccer_control/soccer_pycontrol/setup.py index 19af15fa1..41b9277fa 100644 --- a/soccer_control/soccer_pycontrol/setup.py +++ b/soccer_control/soccer_pycontrol/setup.py @@ -1,6 +1,6 @@ from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from ros2_pkg.python_setup import generate_distutils_setup # fmt: off d = generate_distutils_setup( diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/kick.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/kick.py new file mode 100644 index 000000000..1e73b5d01 --- /dev/null +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/kick.py @@ -0,0 +1,116 @@ +import time +from os.path import expanduser + +import numpy as np +import placo +from ischedule import run_loop, schedule +from placo_utils.visualization import point_viz, robot_frame_viz, robot_viz + +""" +Sigmaban humanoid is moving its legs while looking at a moving ball. +""" + +# Loading the robot +robot_model = "assembly" +model_filename = expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf" +robot = placo.HumanoidRobot(model_filename) + +# Placing the left foot in world origin +robot.set_T_world_frame("left_foot", np.eye(4)) +robot.update_kinematics() + +solver = placo.KinematicsSolver(robot) + +# Retrieving initial position of the feet, com and trunk orientation +T_world_left = robot.get_T_world_frame("left_foot") +T_world_right = robot.get_T_world_frame("right_foot") + +# Creating the viewer +viz = robot_viz(robot) + +# Trunk +com_task = solver.add_com_task(np.array([0.0, 0.0, 0.23])) +com_task.configure("com", "soft", 1.0) + +trunk_orientation_task = solver.add_orientation_task("trunk", np.eye(3)) +trunk_orientation_task.configure("trunk_orientation", "soft", 1.0) + +# Keep left and right foot on the floor +left_foot_task = solver.add_frame_task("left_foot", T_world_left) +left_foot_task.configure("left_foot", "soft", 1.0, 1.0) + +right_foot_task = solver.add_frame_task("right_foot", T_world_right) +right_foot_task.configure("right_foot", "soft", 1.0, 1.0) + +# Look at ball +look_at_ball = solver.add_axisalign_task("camera", np.array([1.0, 0.0, 0.0]), np.array([0.0, 0.0, 1.0])) +look_at_ball.configure("look_ball", "soft", 1.0) + +# Creating a very basic lateral swing and foot rise trajectory +left_foot_z_traj = placo.CubicSpline() +left_foot_z_traj.add_point(0.0, 0.0, 0.0) +left_foot_z_traj.add_point(0.5, 0.05, 0.0) +left_foot_z_traj.add_point(1.0, 0.05, 0.0) +left_foot_z_traj.add_point(1.5, 0.0, 0.0) + +left_foot_x_traj = placo.CubicSpline() +left_foot_x_traj.add_point(0.0, 0.0, 0.0) +left_foot_x_traj.add_point(0.5, -0.05, 0.0) +left_foot_x_traj.add_point(1.0, 0.15, 0.0) +left_foot_x_traj.add_point(1.5, 0.0, 0.0) + +left_foot_y_traj = placo.CubicSpline() +left_foot_y_traj.add_point(0.0, 0.0, 0.0) +left_foot_y_traj.add_point(0.5, 0.0, 0.0) +left_foot_y_traj.add_point(1.0, 0.0, 0.0) +left_foot_y_traj.add_point(1.5, 0.0, 0.0) + +# Regularization task +posture_regularization_task = solver.add_joints_task() +posture_regularization_task.set_joints({dof: 0.0 for dof in robot.joint_names()}) +posture_regularization_task.configure("reg", "soft", 1e-5) + +solver.enable_joint_limits(True) +solver.enable_velocity_limits(True) + +t = 0 +dt = 0.01 +last = 0 +solver.dt = dt +start_t = time.time() +robot.update_kinematics() + + +@schedule(interval=dt) +def loop(): + global t + + # Updating the target + t_mod = t % 2.0 + target = left_foot_task.position().target_world + target[0] = left_foot_x_traj.pos(t_mod) + target[1] = left_foot_y_traj.pos(t_mod) + target[2] = left_foot_z_traj.pos(t_mod) + left_foot_task.position().target_world = target + + # Updating the com target with lateral sinusoidal trajectory + com_task.target_world = np.array([0.0, -0.07, 0.23]) + + # Looking at ball + ball = np.array([0.2, 0.0, 0.0]) + camera_pos = robot.get_T_world_frame("camera")[:3, 3] + look_at_ball.targetAxis_world = ball - camera_pos + + solver.solve(True) + robot.update_kinematics() + + viz.display(robot.state.q) + robot_frame_viz(robot, "trunk") + robot_frame_viz(robot, "camera") + point_viz("com", robot.com_world(), radius=0.025, color=0xAAAAAA) + point_viz("ball", ball, radius=0.05, color=0xDDDDDD) + + t += dt + + +run_loop() diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/placo_joint_tasks.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/placo_joint_tasks.py index 695ded97d..beb8f8f5d 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/placo_joint_tasks.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/placo_joint_tasks.py @@ -7,7 +7,7 @@ from placo_utils.visualization import point_viz, robot_frame_viz, robot_viz robot_model = "bez1" -model_filename = expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf" +model_filename = expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf" robot = placo.HumanoidRobot(model_filename) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/stabilize_phase.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/stabilize_phase.py index fc50185eb..f551bce4f 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/stabilize_phase.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/exp/stabilize_phase.py @@ -21,7 +21,7 @@ def __init__( robot_model: str = "bez1", ): with open( - expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_control/soccer_pycontrol/config/{robot_model}/{robot_model}{sim}.yaml", "r" + expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_control/soccer_pycontrol/config/{robot_model}/{robot_model}{sim}.yaml", "r" ) as file: parameters = yaml.safe_load(file) file.close() diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py index 4aff137ae..e335d99ad 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/bez.py @@ -1,3 +1,4 @@ +import enum from os.path import expanduser import yaml @@ -7,6 +8,60 @@ from soccer_common import Transformation +import numpy as np +import math + +class BezStatusEnum(enum.IntEnum): + BALANCE = 0 + FIND_BALL = 1 + WALK = 2 + FALLEN = 3 + ROTATING = 4 + LOCALIZE = 5 + +class HeadScanner: + def __init__(self, max_yaw=np.pi / 2, min_yaw=-np.pi / 2, step_size=0.01): + self.max_yaw = max_yaw + self.min_yaw = min_yaw + self.step_size = step_size + self.yaw = 0.0 # current angle + self.state = "LOOKING_RIGHT" + self.goalposts_found = 0 + self.max_yaw_reached = False + self.min_yaw_reached = False + self.goalpost_distances = [] + + + def update(self, sees_goal: bool) -> float: + """ + Update head angle based on goalpost detection. + :param sees_goal: whether goalpost is visible in current frame + :return: new head yaw angle + """ + + if sees_goal: + self.goalposts_found += 1 + if self.goalposts_found == 1: + self.state = "LOOKING_LEFT" + elif self.goalposts_found == 2: + self.state = "DONE" + + if self.state == "LOOKING_RIGHT": + self.yaw = min(self.yaw + self.step_size, self.max_yaw) + self.max_yaw_reached = (self.yaw + self.step_size >= self.max_yaw) + elif self.state == "LOOKING_LEFT": + self.yaw = max(self.yaw - self.step_size, self.min_yaw) + self.min_yaw_reached = (self.yaw - self.step_size <= self.min_yaw) + elif self.state == "DONE": + pass # Stop moving + + return self.yaw + + def reset(self): + self.yaw = 0.0 + self.state = "LOOKING_RIGHT" + self.goalposts_found = 0 + class Bez: """ @@ -15,20 +70,94 @@ class Bez: # TODO should create more interfaces so that its easier to access objects, streamline interface - def __init__(self, robot_model: str = "bez1", pose: Transformation = Transformation(), fixed_base: bool = False): + def __init__( + self, + robot_model: str = "bez1", + pose: Transformation = Transformation(), + fixed_base: bool = False, + status: BezStatusEnum = BezStatusEnum.BALANCE, + found_ball: bool = False, + ): self.robot_model = robot_model self.parameters = self.get_parameters() + self._status = status + self._body_status = status + self._head_status = status + self._found_ball = found_ball + self._found_home_side = False + self._home_side_yaw_positive = False + self._Location = [] + self.model = LoadModel(self.robot_model, self.parameters["walking_torso_height"], pose, fixed_base) self.motor_control = MotorControl(self.model.body) - self.sensors = Sensors(self.model.body) + self.sensors = Sensors(self.model.body, self.model.ball) + + self._sway_amplitude = 2 + self.head_scanner = HeadScanner(max_yaw=self._sway_amplitude, min_yaw=-self._sway_amplitude, step_size=0.4) + + @property + def status(self) -> BezStatusEnum: + return self._status + + @status.setter + def status(self, status: BezStatusEnum) -> None: + self._status = status + + @property + def head_status(self) -> BezStatusEnum: + return self._head_status + + @head_status.setter + def head_status(self, status: BezStatusEnum) -> None: + self._head_status = status + + @property + def body_status(self) -> BezStatusEnum: + return self._body_status + + @body_status.setter + def body_status(self, status: BezStatusEnum) -> None: + self._body_status = status + + @property + def found_ball(self) -> bool: + return self._found_ball + + @found_ball.setter + def found_ball(self, status: bool) -> None: + self._found_ball = status + + @property + def found_home_side(self) -> bool: + return self._found_home_side + + @found_home_side.setter + def found_home_side(self, status: bool) -> None: + self._found_home_side = status + + @property + def home_side_yaw_positive(self) -> bool: + return self._home_side_yaw_positive + + @home_side_yaw_positive.setter + def home_side_yaw_positive(self, status: bool) -> None: + self._home_side_yaw_positive = status + + @property + def location(self) -> list: + return self._Location + + @location.setter + def location(self, location: list) -> None: + self._Location = location def get_parameters(self) -> dict: with open( expanduser("~") - + f"/catkin_ws/src/soccerbot/soccer_control/soccer_pycontrol/config/{self.robot_model}/{self.robot_model}_sim_pybullet.yaml", + + f"/ros2_ws/src/soccerbot/soccer_control/soccer_pycontrol/config/{self.robot_model}/{self.robot_model}_sim_pybullet.yaml", "r", ) as file: parameters = yaml.safe_load(file) @@ -46,3 +175,19 @@ def fallen(pitch: float) -> bool: print("Fallen Back") return True return False + + @property + def is_balance(self) -> bool: + return self.status == BezStatusEnum.BALANCE + + @property + def is_find_ball(self) -> bool: + return self.status == BezStatusEnum.FIND_BALL + + @property + def is_walk(self) -> bool: + return self.status == BezStatusEnum.WALK + + @property + def is_fallen(self) -> bool: + return self.status == BezStatusEnum.FALLEN diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py index e2e27be0b..4392c6ff8 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/bez_ros.py @@ -12,7 +12,8 @@ class BezROS(Bez): def __init__(self, ns: str = ""): self.ns = ns - self.robot_model = rospy.get_param("robot_model", "bez1") + self.robot_model = "assembly" # rospy.get_param("robot_model", "assembly") + if rospy.get_param("/use_sim_time", False): sim = "_sim" else: @@ -21,16 +22,17 @@ def __init__(self, ns: str = ""): self.parameters = self.get_parameters(sim) motor_offsets = self.get_motor_names() - motor_names = list(motor_offsets.keys())[1:] - self.motor_control = MotorControlROS(motor_names, ns) + motor_names = list(motor_offsets.keys())[1:] + del motor_offsets["universe"] + self.motor_control = MotorControlROS(motor_offsets, ns) self.sensors = SensorsROS(ns) # TODO fix dupe def get_motor_names(self): urdf_model_path = ( - expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_description/{self.robot_model}" f"_description/urdf/{self.robot_model}.urdf" + expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_description/{self.robot_model}" f"_description/urdf/{self.robot_model}.urdf" ) model = pinocchio.buildModelFromUrdf(urdf_model_path) @@ -44,11 +46,13 @@ def get_motor_names(self): # for name, oMi in zip(model.names, data.oMi): # print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))) # TODO should make a unit test to make sure the data is correct and maybe use pybullet toverify - return {model.names[i]: data.oMi[i].translation.T for i in range(len(model.names))} + return { + model.names[i]: [i - 1, i - 1] for i in range(len(model.names)) + } # {model.names[i]: data.oMi[i].translation.T for i in range(len(model.names))} def get_parameters(self, sim: str) -> dict: with open( - expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_control/soccer_pycontrol/config/{self.robot_model}/{self.robot_model}{sim}.yaml", "r" + expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_control/soccer_pycontrol/config/{self.robot_model}/{self.robot_model}{sim}.yaml", "r" ) as file: parameters = yaml.safe_load(file) file.close() diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/motor_control_ros.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/motor_control_ros.py index b4662c753..79b562080 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/motor_control_ros.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/motor_control_ros.py @@ -3,21 +3,21 @@ import rospy from rospy import ROSException from sensor_msgs.msg import JointState -from soccer_pycontrol.model.motor_control import MotorControl +from soccer_pycontrol.model.motor_control import MotorControl, MotorData class MotorControlROS(MotorControl): def __init__( self, - motor_names: List[str], + motor_names, ns: str = "", ): self.motor_names = motor_names self.numb_of_motors = len(self.motor_names) # Todo make it numpy and add getter and setters - self.configuration = [0.0] * self.numb_of_motors - self.configuration_offset = [0.0] * self.numb_of_motors + self.configuration = MotorData(self.motor_names) + self.configuration_offset = MotorData(self.motor_names) # TODO should separate config to current and target @@ -34,9 +34,9 @@ def set_motor(self) -> None: js.position = [] js.effort = [] angles = self.get_angles() - for i, n in enumerate(self.motor_names): - js.name.append(n) - js.position.append(angles[i]) + for joint in self.motor_names: + js.name.append(joint) + js.position.append(angles[self.motor_names[joint][0]]) try: rospy.loginfo_once("Started Publishing Motors") self.pub_all_motor.publish(js) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/sensors_ros.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/sensors_ros.py index 2ab008e23..82ec820dc 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/sensors_ros.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/model_ros/sensors_ros.py @@ -62,8 +62,23 @@ def get_foot_pressure_sensors(self, floor): pass def get_pose(self): - Transformation(pose=self.odom_msg.pose.pose) - return Transformation(pose=self.odom_msg.pose.pose) + # Transformation(pose=self.odom_msg.pose.pose) + # return Transformation(pose=self.odom_msg.pose.pose) + try: + + time_stamp1 = self.tf_listener.getLatestCommonTime("left_foot", "base_link") + + (trans1, rot) = self.tf_listener.lookupTransform("left_foot", "base_link", time_stamp1) + return Transformation(position=[0,0,trans1[2]], quaternion=rot) + except ( + tf2_py.LookupException, + tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException, + tf2_py.TransformException, + ) as ex: + rospy.logerr_throttle(5, "Unable to find transformation from world to ") + pass def get_ball(self): try: @@ -83,3 +98,38 @@ def get_ball(self): ) as ex: rospy.logerr_throttle(5, "Unable to find transformation from world to ") pass + + def get_height(self): + try: + + time_stamp1 = self.tf_listener.getLatestCommonTime("left_foot", "head") + + (trans1, rot) = self.tf_listener.lookupTransform("left_foot", "head", time_stamp1) + temp = Transformation(position=[0, 0, trans1[2]], quaternion=rot) + temp.orientation_euler = 1*temp.orientation_euler + return temp + except ( + tf2_py.LookupException, + tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException, + tf2_py.TransformException, + ) as ex: + rospy.logerr_throttle(5, "Unable to find transformation from world to ") + return Transformation() + + def get_global_height(self): + try: + time_stamp1 = self.tf_listener.getLatestCommonTime("robot1/base_footprint_gt", "robot1/camera_gt") + + (trans1, rot) = self.tf_listener.lookupTransform("robot1/base_footprint_gt", "robot1/camera_gt", time_stamp1) + return Transformation(position=trans1, quaternion=rot) + except ( + tf2_py.LookupException, + tf.LookupException, + tf.ConnectivityException, + tf.ExtrapolationException, + tf2_py.TransformException, + ) as ex: + rospy.logerr_throttle(5, "Unable to find transformation from world to ") + pass diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/motor_control.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/motor_control.py index 4d7e877ac..71987df78 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/motor_control.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/motor_control.py @@ -6,6 +6,48 @@ from soccer_common.utils import wrapToPi +class MotorData: + def __init__(self, motor_names: dict): + self.motor_names = motor_names + self.data = [0.0] * len(motor_names) + + def __getitem__(self, index): + if isinstance(index, slice): + if type(index.start) is str: + return self.data[self.motor_names[index.start][1] : self.motor_names[index.stop][1] + 1] + else: + return self.data[slice(index.start, index.stop)] + if type(index) is str: + return self.data[self.motor_names[index][1]] + + return self.data[index] + + def __setitem__(self, index, value): + if isinstance(index, slice): + if type(index.start) is str: + self.data[self.motor_names[index.start][1] : self.motor_names[index.stop][1] + 1] = value + else: + self.data[slice(index.start, index.stop)] = value + else: + if type(index) is str: + self.data[self.motor_names[index][1]] = value + else: + self.data[index] = value + + def reset(self): + self.data = [0.0] * len(self.motor_names) + + +# x = {"1": [0,0], "2":[1,1], "3":[2,2] } # TODO do analysis if this hurts performance +# m = MotorData(x) +# print(m["1":"2"]) +# print(m["1"]) +# print(m["2"]) +# m["1"] = 4 +# m["1":"2"] = [3,4] +# print(m[:]) +# m[:] = 0 +# print("end", m[:]) class MotorControl: """ Class controls access to motor information and sets motor angles in pybullet @@ -15,82 +57,84 @@ class MotorControl: def __init__(self, body: pb.loadURDF): self.body = body - self.motor_names = self.find_motor_names() - + self.motor_names = self.find_motor_names() # motorname : [pybullet index, array index] self.numb_of_motors = len(self.motor_names) # Todo make it numpy and add getter and setters - self.configuration = [0.0] * self.numb_of_motors - self.configuration_offset = [0.0] * self.numb_of_motors + self.configuration = MotorData(self.motor_names) + self.configuration_offset = MotorData(self.motor_names) self.max_forces = [] - for i in range(0, self.numb_of_motors): + for i in range(0, self.numb_of_motors): # TODO change self.max_forces.append(pb.getJointInfo(self.body, i)[10] or 6) # TODO why is this acting so weird self.set_motor() - def find_motor_names(self) -> List[str]: - - names = [] + def find_motor_names(self) -> dict: + names = {} for i in range(pb.getNumJoints(self.body)): joint_info = pb.getJointInfo(self.body, i) if joint_info[2] == pb.JOINT_REVOLUTE: - names.append(joint_info[1].decode("utf-8")) + names[joint_info[1].decode("utf-8")] = [i, len(names)] return names + def get_motor_pybullet_indexes(self) -> List[float]: + return [i[0] for i in list(self.motor_names.values())] + + def get_motor_indexes(self) -> List[float]: + return [i[1] for i in list(self.motor_names.values())] + def get_angles(self): """ Function for getting all the angles, combines the configuration with the configuration offset :return: All 18 angles of the robot in an array formation """ - angles = [wrapToPi(a + b) for a, b in zip(self.configuration, self.configuration_offset)] + # TODO is this needed + angles = [wrapToPi(a + b) for a, b in zip(self.configuration.data, self.configuration_offset.data)] return angles def set_motor(self) -> None: pb.setJointMotorControlArray( bodyIndex=self.body, controlMode=pb.POSITION_CONTROL, - jointIndices=list(range(0, self.numb_of_motors, 1)), + jointIndices=self.get_motor_pybullet_indexes(), # list(range(0, self.numb_of_motors, 1)), targetPositions=self.get_angles(), forces=self.max_forces, ) - def reset_target_angles(self): - self.configuration[:] = [0.0 for _ in self.configuration] - - def set_target_angles(self, target_angles: np.ndarray) -> None: - self.configuration[:] = target_angles + def set_single_motor(self,name, target_angle: float) -> None: + self.configuration[name] = target_angle def set_head_target_angles(self, target_angles: np.ndarray) -> None: - self.configuration[self.motor_names.index("head_yaw") : self.motor_names.index("head_pitch") + 1] = target_angles + self.configuration["head_yaw":"head_pitch"] = target_angles def set_right_arm_target_angles(self, target_angles: np.ndarray) -> None: - self.configuration[self.motor_names.index("right_shoulder_pitch") : self.motor_names.index("right_elbow") + 1] = target_angles + self.configuration["right_shoulder_pitch":"right_elbow"] = target_angles def set_left_arm_target_angles(self, target_angles: np.ndarray) -> None: - self.configuration[self.motor_names.index("left_shoulder_pitch") : self.motor_names.index("left_elbow") + 1] = target_angles + self.configuration["left_shoulder_pitch":"left_elbow"] = target_angles def set_right_leg_target_angles(self, target_angles: np.ndarray) -> None: - self.configuration[self.motor_names.index("right_hip_yaw") : self.motor_names.index("right_ankle_roll") + 1] = target_angles + self.configuration["right_hip_yaw":"right_ankle_roll"] = target_angles def set_left_leg_target_angles(self, target_angles: np.ndarray) -> None: - self.configuration[self.motor_names.index("left_hip_yaw") : self.motor_names.index("left_ankle_roll") + 1] = target_angles + self.configuration["left_hip_yaw":"left_ankle_roll"] = target_angles def set_leg_joint_2_target_angle(self, target: float) -> None: - self.configuration_offset[self.motor_names.index("left_hip_roll")] -= target - self.configuration_offset[self.motor_names.index("right_hip_roll")] += target + self.configuration_offset["left_hip_roll"] = -target + self.configuration_offset["right_hip_roll"] = +target def set_leg_joint_3_target_angle(self, target: float) -> None: - self.configuration_offset[self.motor_names.index("left_hip_pitch")] = +target - self.configuration_offset[self.motor_names.index("right_hip_pitch")] = +target + self.configuration_offset["left_hip_pitch"] = target + self.configuration_offset["right_hip_pitch"] = target def set_leg_joint_5_target_angle(self, target: float) -> None: - self.configuration_offset[self.motor_names.index("left_ankle_pitch")] = target - self.configuration_offset[self.motor_names.index("right_ankle_pitch")] = target + self.configuration_offset["left_ankle_pitch"] = target + self.configuration_offset["right_ankle_pitch"] = target def set_leg_joint_6_target_angle(self, target: float) -> None: - self.configuration_offset[self.motor_names.index("left_ankle_roll")] -= target - self.configuration_offset[self.motor_names.index("right_ankle_roll")] += target + self.configuration_offset["left_ankle_roll"] -= target + self.configuration_offset["right_ankle_roll"] += target diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py index d9ec04eb6..77bc24b3e 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/model/sensors.py @@ -1,5 +1,6 @@ from typing import List +import cv2 import numpy as np import pybullet as pb @@ -11,9 +12,10 @@ class Sensors: Interfaces with pybullet to extract sensor data. """ - def __init__(self, body: pb.loadURDF): + def __init__(self, body: pb.loadURDF, ball: pb.loadURDF): # TODO does this need to be a class? self.body = body + self.ball = ball # TODO should get based on name self.imu_link = pb.getNumJoints(self.body) - 1 self.imu_ready = False @@ -43,7 +45,14 @@ def get_imu(self) -> Transformation: return Transformation(pos, orientation).orientation_euler def get_ball(self): - return Transformation() + [position, quaternion] = pb.getBasePositionAndOrientation(self.body) + [ball_position, ball_quaternion] = pb.getBasePositionAndOrientation(self.ball) + trans = (np.array(ball_position) - np.array(position)).tolist() + return Transformation(position=trans, quaternion=ball_quaternion) + + def get_ball_global(self): + [ball_position, ball_quaternion] = pb.getBasePositionAndOrientation(self.ball) + return Transformation(position=ball_position, quaternion=ball_quaternion) def get_foot_pressure_sensors(self, floor: pb.loadURDF) -> List[bool]: """ @@ -86,31 +95,36 @@ def get_camera_image(self): """ Captures the image from the camera mounted on the robot """ - robot_position, robot_orientation = pb.getBasePositionAndOrientation(self.body) - # Add more offsets later - camera_position = robot_position - camera_target = [robot_position[0] + 1, robot_position[1], robot_position[2]] - - camera_up = [0, 0, 1] - - view_matrix = pb.computeViewMatrix(camera_position, camera_target, camera_up) - - width, height = 1280, 720 - fov = 60 + camera_position = self.get_pose(link=2).position + + # print(f"Pos: {camera_position} Orient: {self.get_pose(link=2).orientation_euler}") + view_matrix = pb.computeViewMatrixFromYawPitchRoll( + camera_position, + 0.000367, + pb.getJointState(self.body, 0)[0] * (180 / np.pi) - 90, # self.get_pose(link=2).orientation_euler[0]*(180/np.pi) + 90, + -pb.getJointState(self.body, 1)[0] * (180 / np.pi), # self.get_pose(link=2).orientation_euler[1]*(180/np.pi)+90, + 0, # self.get_pose(link=2).orientation_euler[2]*(180/np.pi) , + 2, + ) + width, height = 640, 480 + fov = 78 aspect = width / height - near = 0.1 - far = 50 + near = 0.2 + far = 100 projection_matrix = pb.computeProjectionMatrixFOV(fov, aspect, near, far) - images = pb.getCameraImage(width, height, view_matrix, projection_matrix, shadow=False, - renderer=pb.ER_BULLET_HARDWARE_OPENGL) + images = pb.getCameraImage(width, height, view_matrix, projection_matrix, shadow=False, renderer=pb.ER_BULLET_HARDWARE_OPENGL) # NOTE: the ordering of height and width change based on the conversion - rgb_opengl = np.reshape(images[2], (height, width, 4))[:, :, :3] * 1. / 255. + img = np.reshape(images[2], (height, width, 4)) + img = cv2.cvtColor(img, cv2.COLOR_RGBA2BGR) + # rgb_opengl = np.reshape(images[2], (height, width, 4))[:, :, :3] * 1. / 255. # depth_buffer_opengl = np.reshape(images[3], [width, height]) # depth_opengl = far * near / (far - (far - near) * depth_buffer_opengl) # seg_opengl = np.reshape(images[4], [width, height]) * 1. / 255. - return rgb_opengl \ No newline at end of file + return img + def get_height(self): + return \ No newline at end of file diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py index ac486bb78..651eb1cc8 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_load_model.py @@ -15,19 +15,24 @@ def __init__(self, robot_model: str, walking_torso_height: float, pose: Transfor self.pose = pose self.robot_model = robot_model - urdf_model_path = expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_description/{robot_model}" f"_description/urdf/{robot_model}.urdf" + urdf_model_path = expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_description/{robot_model}" f"_description/urdf/{robot_model}.urdf" self.body = self.load_urdf_pybullet(urdf_model_path, fixed_base) self.walking_torso_height = walking_torso_height - if not fixed_base and (self.pose == Transformation()).all(): # TODO need way to have custom height - self.set_pose(pose) + # if not fixed_base and (self.pose == Transformation()).all(): # TODO need way to have custom height + self.set_pose(pose) + # self.ball = pb.loadURDF("soccerball.urdf", [0.12, 0.05, 0.1], globalScaling=0.14) + # self.ball = pb.loadURDF("soccerball.urdf", [0.23, 0.07, 0.07], globalScaling=0.14) + self.ball = pb.loadURDF("soccerball.urdf", [1, 0.07, 0.07], globalScaling=0.14) + pb.changeDynamics(self.ball, -1, mass=0.2, linearDamping=0, angularDamping=0, rollingFriction=0.001, spinningFriction=0.001) + pb.changeVisualShape(self.ball, -1, rgbaColor=[0.8, 0.8, 0.8, 1]) def load_urdf_pybullet(self, urdf_model_path: str, fixed_base: bool): # -> pb.loadURDF: # TODO read from yaml? Also maybe put in world body = pb.loadURDF( urdf_model_path, useFixedBase=fixed_base, - flags=pb.URDF_USE_INERTIA_FROM_FILE | 0, + flags=pb.URDF_USE_INERTIA_FROM_FILE | pb.URDF_ENABLE_CACHED_GRAPHICS_SHAPES | 0, basePosition=self.pose.position, baseOrientation=self.pose.quaternion, ) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py index af232796a..a1d80d9ba 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/pybullet_usage/pybullet_world.py @@ -32,17 +32,22 @@ def __init__( """ self.rate = rate self.real_time = real_time - + optionstring = "--width={} --height={} ".format(640, 480) assert pb.isConnected() == 0 if display: - self.client_id = pb.connect(pb.GUI) + self.client_id = pb.connect(pb.GUI, optionstring) else: self.client_id = pb.connect(pb.DIRECT) + pb.setTimeStep(1.0 / rate) + # pb.setRealTimeSimulation(0) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally pb.resetDebugVisualizerCamera(cameraDistance=1.0, cameraYaw=camera_yaw, cameraPitch=0, cameraTargetPosition=cameraTargetPosition) pb.setGravity(0, 0, -9.81) pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_RGB_BUFFER_PREVIEW, 0) if path != "": self.plane = pb.loadURDF(path, basePosition=position, baseOrientation=pb.getQuaternionFromEuler(orientation)) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py index aeadab658..7eb3a8c75 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/foot_step_planner.py @@ -9,13 +9,18 @@ class FootStepPlanner: - def __init__(self, robot_model: str, parameters: dict, funct_time, debug: bool = True): + def __init__(self, robot_model: str, parameters: dict, funct_time, debug: bool = True, ball: bool = False, sim: bool = True): + self.ball = ball self.funct_time = funct_time self.DT = parameters["control_frequency"] self.debug = debug self.robot_model = robot_model - model_filename = expanduser("~") + f"/catkin_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf" + if sim and self.robot_model == "assembly": + model_filename = expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf" + + else: + model_filename = expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot1.urdf" self.parameters = self.walk_parameters(parameters) self.last_replan = 0 @@ -82,48 +87,73 @@ def setup_tasks(self): shoulder_roll = 0 * np.pi / 180 shoulder_pitch = -0.45 # 20 * np.pi / 180 joints_task = self.solver.add_joints_task() - if self.robot_model == "bez2" or self.robot_model == "assembly": - joints_task.set_joints( - { - "left_shoulder_roll": shoulder_roll, - "left_shoulder_pitch": shoulder_pitch, - "left_elbow": elbow, - "right_shoulder_roll": -shoulder_roll, - "right_shoulder_pitch": shoulder_pitch, - "right_elbow": elbow, - "head_pitch": 0.0, - "head_yaw": 0.0, - } - ) + if self.ball: + self.look_at_ball = self.solver.add_axisalign_task("camera", np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0])) + self.look_at_ball.configure("look_ball", "soft", 0.5) # TODO replace with a function that remove_task + if self.robot_model == "bez2" or self.robot_model == "assembly": + joints_task.set_joints( + { + "left_shoulder_roll": shoulder_roll, + "left_shoulder_pitch": shoulder_pitch, + "left_elbow": elbow, + "right_shoulder_roll": -shoulder_roll, + "right_shoulder_pitch": shoulder_pitch, + "right_elbow": elbow, + # "head_pitch": 0.0, + # "head_yaw": 0.0, + } + ) + else: + joints_task.set_joints( + { + # "left_shoulder_roll": shoulder_roll, + "left_shoulder_pitch": shoulder_pitch, + "left_elbow": elbow, + # "right_shoulder_roll": -shoulder_roll, + "right_shoulder_pitch": shoulder_pitch, + "right_elbow": elbow, + # "head_pitch": 0.0, + # "head_yaw": 0.0, + } + ) else: - joints_task.set_joints( - { - # "left_shoulder_roll": shoulder_roll, - "left_shoulder_pitch": shoulder_pitch, - "left_elbow": elbow, - # "right_shoulder_roll": -shoulder_roll, - "right_shoulder_pitch": shoulder_pitch, - "right_elbow": elbow, - "head_pitch": 0.0, - "head_yaw": 0.0, - } - ) + if self.robot_model == "bez2" or self.robot_model == "assembly": + joints_task.set_joints( + { + "left_shoulder_roll": shoulder_roll, + "left_shoulder_pitch": shoulder_pitch, + "left_elbow": elbow, + "right_shoulder_roll": -shoulder_roll, + "right_shoulder_pitch": shoulder_pitch, + "right_elbow": elbow, + "head_pitch": 0.0, + "head_yaw": 0.0, + } + ) + else: + joints_task.set_joints( + { + # "left_shoulder_roll": shoulder_roll, + "left_shoulder_pitch": shoulder_pitch, + "left_elbow": elbow, + # "right_shoulder_roll": -shoulder_roll, + "right_shoulder_pitch": shoulder_pitch, + "right_elbow": elbow, + "head_pitch": 0.0, + "head_yaw": 0.0, + } + ) joints_task.configure("joints", "soft", 1.0) - # self.look_at_ball = self.solver.add_axisalign_task( - # "camera", np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0]) - # ) - # self.look_at_ball.configure("look_ball", "soft", 1) - # Placing the robot in the initial position - print("Placing the robot in the initial position...") + # print("Placing the robot in the initial position...") self.tasks.reach_initial_pose( np.eye(4), self.parameters.feet_spacing, self.parameters.walk_com_height, self.parameters.walk_trunk_pitch, ) - print("Initial position reached") + # print("Initial position reached") def configure_planner(self, d_x: float = 0.0, d_y: float = 0.0, d_theta: float = 0.0, nb_steps: int = 10): # Configure the FootstepsPlanner @@ -161,7 +191,9 @@ def head_movement(self, target: List[float]): # TODO clean up and add a cone or it breaks walking ball = np.array(target) camera_pos = self.robot.get_T_world_frame("camera")[:3, 3] - ball[2] -= camera_pos[2] + ball[2] -= camera_pos[2] # TODO tune later + # ball[1] = -ball[1] + self.look_at_ball.targetAxis_world = ball def plan_steps( diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/kick_planner.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/kick_planner.py new file mode 100644 index 000000000..797bc99eb --- /dev/null +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/kick_planner.py @@ -0,0 +1,194 @@ +import time +from os.path import expanduser +from typing import List + +import numpy as np +import placo +import pybullet as p +from placo_utils.visualization import ( + footsteps_viz, + frame_viz, + line_viz, + point_viz, + robot_frame_viz, + robot_viz, +) + + +class KickPlanner: + def __init__(self, robot_model: str, parameters: dict, funct_time, debug: bool = True): + self.funct_time = funct_time + self.DT = parameters["control_frequency"] + + self.debug = debug + self.robot_model = robot_model + model_filename = expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/robot.urdf" + + self.start_t = self.funct_time() + # TODO is there too many global var + self.robot = placo.HumanoidRobot(model_filename) + self.t = 0 + + self.solver = None + if self.debug: + # Starting Meshcat viewer + self.viz = robot_viz(self.robot) + self.last_display = self.funct_time() + + # TODO can this be in init + def setup_tasks(self): + # Creating the kinematics solver + self.robot.set_T_world_frame("left_foot", np.eye(4)) + self.robot.update_kinematics() + + self.solver = placo.KinematicsSolver(self.robot) + self.solver.enable_velocity_limits(True) + self.solver.enable_joint_limits(True) + self.solver.dt = self.DT + + T_world_left = self.robot.get_T_world_frame("left_foot") + T_world_right = self.robot.get_T_world_frame("right_foot") + + self.com_task = self.solver.add_com_task(np.array([0.0, 0.0, 0.23])) + self.com_task.configure("com", "hard", 1.0) + + trunk_orientation_task = self.solver.add_orientation_task("trunk", np.eye(3)) + trunk_orientation_task.configure("trunk_orientation", "soft", 1.0) + + # Keep left and right foot on the floor + self.left_foot_task = self.solver.add_frame_task("left_foot", T_world_left) + self.left_foot_task.configure("left_foot", "soft", 1.0, 1.0) + + self.right_foot_task = self.solver.add_frame_task("right_foot", T_world_right) + self.right_foot_task.configure("right_foot", "soft", 1.0, 1.0) + + # Creating a very basic lateral swing and foot rise trajectory + ti = [0.1, 0.2, 0.3, 0.4] + self.left_foot_z_traj = placo.CubicSpline() + self.left_foot_z_traj.add_point(ti[0], 0.0, 0.0) + self.left_foot_z_traj.add_point(ti[1], 0.05, 0.0) + self.left_foot_z_traj.add_point(ti[2], 0.075, 0.0) + self.left_foot_z_traj.add_point(ti[3], 0.0, 0.0) + + self.left_foot_x_traj = placo.CubicSpline() + self.left_foot_x_traj.add_point(ti[0], 0.0, 0.0) + self.left_foot_x_traj.add_point(ti[1], -0.05, 0.0) + self.left_foot_x_traj.add_point(ti[2], 0.5, 0.0) + self.left_foot_x_traj.add_point(ti[3], 0.0, 0.0) + + self.left_foot_y_traj = placo.CubicSpline() + self.left_foot_y_traj.add_point(ti[0], 0.0, 0.0) + self.left_foot_y_traj.add_point(ti[1], 0.0, 0.0) + self.left_foot_y_traj.add_point(ti[2], 0.0, 0.0) + self.left_foot_y_traj.add_point(ti[3], 0.0, 0.0) + + self.com_z_traj = placo.CubicSpline() + self.com_z_traj.add_point(ti[0], 0.23, 0.0) + self.com_z_traj.add_point(ti[1], 0.20, 0.0) + self.com_z_traj.add_point(ti[2], 0.17, 0.0) + self.com_z_traj.add_point(ti[3], 0.23, 0.0) + + self.com_x_traj = placo.CubicSpline() + self.com_x_traj.add_point(ti[0], 0.0, 0.0) + self.com_x_traj.add_point(ti[1], 0, 0.0) + self.com_x_traj.add_point(ti[2], 0, 0.0) + self.com_x_traj.add_point(ti[3], 0.0, 0.0) + + self.com_y_traj = placo.CubicSpline() + self.com_y_traj.add_point(ti[0], -0.06, 0.0) + self.com_y_traj.add_point(ti[1], -0.08, 0.0) + self.com_y_traj.add_point(ti[2], -0.08, 0.0) + self.com_y_traj.add_point(ti[3], -0.04, 0.0) + + # Regularization task + posture_regularization_task = self.solver.add_joints_task() + posture_regularization_task.set_joints({dof: 0.0 for dof in self.robot.joint_names()}) + posture_regularization_task.configure("reg", "soft", 1e-5) + + # Creating a joint task to assign DoF values for upper body + elbow = 2.512 # -50 * np.pi / 180 + shoulder_roll = 0 * np.pi / 180 + shoulder_pitch = -0.45 # 20 * np.pi / 180 + joints_task = self.solver.add_joints_task() + if self.robot_model == "bez2" or self.robot_model == "assembly": + joints_task.set_joints( + { + # "left_shoulder_roll": shoulder_roll, + # "left_shoulder_pitch": shoulder_pitch, + "left_elbow": elbow, + # "right_shoulder_roll": -shoulder_roll, + # "right_shoulder_pitch": shoulder_pitch, + "right_elbow": elbow, + # "head_pitch": 0.0, + # "head_yaw": 0.0, + } + ) + else: + joints_task.set_joints( + { + # "left_shoulder_roll": shoulder_roll, + # "left_shoulder_pitch": shoulder_pitch, + "left_elbow": elbow, + # "right_shoulder_roll": -shoulder_roll, + # "right_shoulder_pitch": shoulder_pitch, + "right_elbow": elbow, + # "head_pitch": 0.0, + # "head_yaw": 0.0, + } + ) + joints_task.configure("joints", "soft", 1.0) + + self.look_at_ball = self.solver.add_axisalign_task("camera", np.array([1.0, 0.0, 0.0]), np.array([1.0, 0.0, 0.0])) + self.look_at_ball.configure("look_ball", "soft", 1) # TODO replace with a function that remove_task + + def head_movement(self, target: List[float]): + # TODO clean up and add a cone or it breaks walking + ball = np.array(target) + camera_pos = self.robot.get_T_world_frame("camera")[:3, 3] + ball[2] -= camera_pos[2] + self.look_at_ball.targetAxis_world = ball + + def plan_kick(self, t: float): + t_mod = t % 2.0 + target = self.left_foot_task.position().target_world + target[0] = self.left_foot_x_traj.pos(t_mod) + target[1] = self.left_foot_y_traj.pos(t_mod) + target[2] = self.left_foot_z_traj.pos(t_mod) + self.left_foot_task.position().target_world = target + + # Updating the com target with lateral sinusoidal trajectory + target2 = self.com_task.target_world + target2[0] = self.com_x_traj.pos(t_mod) + target2[1] = self.com_y_traj.pos(t_mod) + target2[2] = self.com_z_traj.pos(t_mod) + self.com_task.target_world = target2 + + # Looking at ball + ball = [0.2, 0.0, 0.0] + self.head_movement(ball) + + self.solver.solve(True) + self.robot.update_kinematics() + self.update_viz(ball) + + def step(self, t: float): + # Spin-lock until the next tick + t += self.DT + # while self.funct_time() < self.start_t + t: + # time.sleep(1e-3) + + return t + + def update_viz(self, ball): + if self.debug: + self.viz.display(self.robot.state.q) + robot_frame_viz(self.robot, "trunk") + robot_frame_viz(self.robot, "camera") + point_viz("com", self.robot.com_world(), radius=0.025, color=0xAAAAAA) + point_viz("ball", ball, radius=0.05, color=0xDDDDDD) + + def is_done(self, t: float) -> bool: + return t >= 0.4 + +# if __name__ == "__main__": +# walk = FootStepPlanner("bez1", time.time, debug=True) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py index 70a52371e..ba59f5745 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/navigator.py @@ -9,22 +9,35 @@ from soccer_pycontrol.model.bez import Bez from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld from soccer_pycontrol.walk_engine.foot_step_planner import FootStepPlanner +from soccer_pycontrol.walk_engine.kick_planner import KickPlanner from soccer_pycontrol.walk_engine.stabilize import Stabilize from soccer_common import PID, Transformation + # TODO could make it more modular by passing in pybullet stuff or have it at one layer higher so we can reuse code # TODO change to trajectory controller class Navigator: - def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = False, record_walking_metrics: bool = True): + def __init__( + self, + world: PybulletWorld, + bez: Bez, + imu_feedback_enabled: bool = False, + ball: bool = False, + record_walking_metrics: bool = True, + sim: bool = True, + ): + self.ball_dx = 0 + self.ball_dy = 0.7 self.world = world self.bez = bez self.imu_feedback_enabled = imu_feedback_enabled self.func_step = self.world.step - self.foot_step_planner = FootStepPlanner(self.bez.robot_model, self.bez.parameters, time.time) - + self.foot_step_planner = FootStepPlanner(self.bez.robot_model, self.bez.parameters, time.time, ball=ball, sim=sim) + self.ball2 = ball + self.kick_planner = KickPlanner(self.bez.robot_model, self.bez.parameters, time.time) self.walk_pid = Stabilize(self.bez.parameters) - self.max_vel = 0.02 + self.max_vel = 0.1 self.nav_x_pid = PID( Kp=0.5, Kd=0, @@ -33,44 +46,81 @@ def __init__(self, world: PybulletWorld, bez: Bez, imu_feedback_enabled: bool = output_limits=(-self.max_vel, self.max_vel), ) self.nav_y_pid = PID( # TODO properly tune later - Kp=0.1, + Kp=0.5, Kd=0, Ki=0, setpoint=0, - output_limits=(-self.max_vel + 0.01, self.max_vel), + output_limits=(-self.max_vel, self.max_vel), ) # TODO could also mod if balance is decreasing self.nav_yaw_pid = PID( - Kp=0.05, + Kp=0.2, Kd=0, Ki=0, setpoint=0, output_limits=(-1, 1), ) - - self.error_tol = 0.01 # in m TODO add as a param and in the ros version + self.last_ball = [0, 0] + self.error_tol = 0.03 # in m TODO add as a param and in the ros version # joints - self.left_ankle_index = self.bez.motor_control.motor_names.index("left_ankle_roll") - self.right_ankle_index = self.bez.motor_control.motor_names.index("right_ankle_roll") + self.left_ankle_index = self.bez.motor_control.motor_names["left_ankle_roll"] + self.right_ankle_index = self.bez.motor_control.motor_names["right_ankle_roll"] # self.torso_index = self.bez.motor_control.body. self.record_walking_metrics = record_walking_metrics self.walking_data = defaultdict(list) + self.t = None + self.enable_walking = None + self.reset_walk() + self.t2 = 0 + + # self.ball_x_pid = PID( + # Kp=0.00001, + # Kd=0, + # Ki=0, + # setpoint=320, + # output_limits=(-1.5707963267948966, 1.5707963267948966), + # ) + # + # self.ball_y_pid = PID( + # Kp=-0.0, + # Kd=0, + # Ki=0, + # setpoint=240, + # output_limits=(0, 1), + # ) + + def reset_walk(self): + self.t = -1 + self.enable_walking = True + + def kick(self): + self.kick_planner.plan_kick(self.t2) + self.set_angles_from_placo(self.kick_planner) + + self.bez.motor_control.set_motor() + + self.t2 = self.kick_planner.step(self.t2) + + def kick_finished(self): + return self.kick_planner.is_done(self.t2) # TODO could make input a vector - def walk(self, target_goal: Union[Transformation, List], ball_mode: bool = False, display_metrics: bool = False): - if isinstance(target_goal, Transformation): - if ball_mode: - self.walk_ball(target_goal) - else: - self.walk_pose(target_goal) - elif isinstance(target_goal, list): # [d_x: float = 0.0, d_y: float = 0.0, d_theta: float = 0.0, nb_steps: int = 10, t_goal: float = 10] - self.walk_time(target_goal) + def walk(self, target_goal: Union[Transformation, List], ball_pixel: list = (), ball_mode: bool = False, display_metrics: bool = False): + if self.enable_walking: + if isinstance(target_goal, Transformation): + if ball_mode: + self.walk_ball(target_goal, ball_pixel) + else: + self.walk_pose(target_goal) + elif isinstance(target_goal, list): # [d_x: float = 0.0, d_y: float = 0.0, d_theta: float = 0.0, nb_steps: int = 10, t_goal: float = 10] + print("reached") + self.walk_time(target_goal) # if self.record_walking_metrics and display_metrics: # self.display_walking_metrics(show_targets=isinstance(target_goal, Transformation)) - self.ready() + # self.ready() def find_new_vel(self, goal_loc: list, curr_loc: list = (0, 0)): x_error = abs(goal_loc[0] - curr_loc[0]) @@ -89,27 +139,29 @@ def find_new_vel(self, goal_loc: list, curr_loc: list = (0, 0)): return math.copysign(dx, goal_loc[0]), math.copysign(dy, goal_loc[1]) def walk_pose(self, target_goal: Transformation): - self.walk_pid.reset_imus() - self.nav_x_pid.reset() - self.nav_x_pid.setpoint = target_goal.position[0] + pose = self.bez.sensors.get_pose() # can use self.foot_step_planner.trajectory.get_p_world_CoM(t) + + if self.t < 0: + self.walk_pid.reset_imus() + self.nav_x_pid.reset() + self.nav_x_pid.setpoint = target_goal.position[0] - self.nav_y_pid.reset() - self.nav_y_pid.setpoint = target_goal.position[1] + self.nav_y_pid.reset() + self.nav_y_pid.setpoint = target_goal.position[1] - self.nav_yaw_pid.reset() - self.nav_yaw_pid.setpoint = target_goal.orientation_euler[0] + self.nav_yaw_pid.reset() + self.nav_yaw_pid.setpoint = target_goal.orientation_euler[0] - pose = self.bez.sensors.get_pose() # can use self.foot_step_planner.trajectory.get_p_world_CoM(t) - dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) - # he = self.heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0]) - # dtheta = math.copysign(0.5, he) - self.foot_step_planner.setup_walk(dx, dy) - t = 0 - # TODO fix and add to a nav could add a funct for pybullet or python - # TODO could have a balancing mode by default could use the COM - # TODO for ball could just remove - - while ( + dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) + # he = self.heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0]) + # dtheta = math.copysign(0.5, he) + self.foot_step_planner.setup_walk(dx, dy) + self.t = 0 + # TODO fix and add to a nav could add a funct for pybullet or python + # TODO could have a balancing mode by default could use the COM + # TODO for ball could just remove + + if ( self.position_error(pose.position[:2], target_goal.position[:2]) > self.error_tol or abs(self.heading_error(target_goal.orientation_euler[0], pose.orientation_euler[0])) > self.error_tol ): # self.bez.sensors.get_pose() #TODO about 20% or 40% error @@ -118,7 +170,7 @@ def walk_pose(self, target_goal: Transformation): ) # self.foot_step_planner.robot.get_T_world_trunk() # can use self.foot_step_planner.trajectory.get_p_world_CoM(t) # TODO should be broken up and a unit test - print(self.foot_step_planner.robot.com_world()) + # print(self.foot_step_planner.robot.com_world()) goal = Transformation() goal.rotation_matrix = np.matmul(target_goal.rotation_matrix, scipy.linalg.inv(pose.rotation_matrix)) goal.position = pose.rotation_matrix.T @ target_goal.position - pose.rotation_matrix.T @ pose.position @@ -135,40 +187,39 @@ def walk_pose(self, target_goal: Transformation): dtheta = self.nav_yaw_pid.update(pose.orientation_euler[0]) print(round(dx, 3), " ", round(dy, 3), " ", round(dtheta, 3), " ", round(x_error, 3), " ", round(y_error, 3), " ", round(head_error, 3)) self.foot_step_planner.configure_planner(dx, dy, dtheta) + self.walk_loop(target_goal) # TODO move main loop out of here + else: + self.ready() + self.enable_walking = False - # if t > 5: - # - # target_goal = Transformation(position=[-0.5,-0.5,0]) - # dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2], curr_loc=position[:2]) - # self.foot_step_planner.configure_planner(dx, dy) - - t = self.walk_loop(t) - - def walk_ball(self, target_goal: Transformation): - self.walk_pid.reset_imus() - self.nav_x_pid.reset() - self.nav_x_pid.setpoint = target_goal.position[0] - - self.nav_y_pid.reset() - self.nav_y_pid.setpoint = target_goal.position[1] - - self.nav_yaw_pid.reset() - self.nav_yaw_pid.setpoint = 0 # TODO add yaw modes - - dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) - # dx, dy = 0, 0 - self.foot_step_planner.setup_walk(dx, dy) - t = 0 - # TODO fix and add to a nav could add a funct for pybullet or python - # TODO could have a balancing mode by default could use the COM - # TODO for ball could just remove - while ( + def walk_ball(self, target_goal: Transformation, ball_pixel: list): + if self.t < 0: + self.walk_pid.reset_imus() + # self.ball_x_pid.reset() + # self.ball_y_pid.reset() + + self.nav_x_pid.reset() + self.nav_x_pid.setpoint = target_goal.position[0] + + self.nav_y_pid.reset() + self.nav_y_pid.setpoint = target_goal.position[1] + + self.nav_yaw_pid.reset() + self.nav_yaw_pid.setpoint = 0 # TODO add yaw modes + + dx, dy = self.find_new_vel(goal_loc=target_goal.position[:2]) + # dx, dy = 0, 0 + self.foot_step_planner.setup_walk(dx, dy) + self.t = 0 + # TODO fix and add to a nav could add a funct for pybullet or python + # TODO could have a balancing mode by default could use the COM + # TODO for ball could just remove + if ( self.position_error(target_goal.position[:2]) > self.error_tol or abs(self.heading_error(target_goal.orientation_euler[0], self.bez.sensors.get_pose().orientation_euler[0])) > self.error_tol ): - target_goal = self.bez.sensors.get_ball() - - # self.foot_step_planner.head_movement(target_goal.position) + # target_goal = self.bez.sensors.get_ball() + # print(target_goal.position) self.nav_x_pid.setpoint = target_goal.position[0] self.nav_y_pid.setpoint = target_goal.position[1] @@ -182,31 +233,56 @@ def walk_ball(self, target_goal: Transformation): dy = self.nav_y_pid.update(0) dtheta = self.nav_yaw_pid.update(self.bez.sensors.get_pose().orientation_euler[0]) - print(round(dx, 3), " ", round(dy, 3), " ", round(dtheta, 3), " ", round(x_error, 3), " ", round(y_error, 3), " ", round(head_error, 3)) + # print(round(dx, 3), " ", round(dy, 3), " ", round(dtheta, 3), " ", round(x_error, 3), " ", round(y_error, 3), " ", round(head_error, 3)) self.foot_step_planner.configure_planner(dx, dy, dtheta) - t = self.walk_loop(t) + self.walk_loop(target_goal) + # else: + # self.ready() + # self.enable_walking = False def walk_time(self, target_goal: list): - self.foot_step_planner.setup_walk(target_goal[0], target_goal[1], target_goal[2], target_goal[3]) - self.walk_pid.reset_imus() - t = 0 - while t < target_goal[4]: - # self.foot_step_planner.head_movement(self.bez.sensors.get_ball().position) - t = self.walk_loop(t) - - def walk_loop(self, t: float): - self.foot_step_planner.plan_steps(t) - self.bez.motor_control.configuration = self.filter_joints() - + if self.t < 0: + self.foot_step_planner.setup_walk(target_goal[0], target_goal[1], target_goal[2], target_goal[3]) + self.walk_pid.reset_imus() + self.t = 0 + + if self.t < target_goal[4]: + self.walk_loop(target_goal) + elif target_goal[4] <= self.t: + self.ready() + self.enable_walking = False + + def walk_loop(self, target_goal: Transformation): + self.foot_step_planner.plan_steps(self.t) + self.set_angles_from_placo(self.foot_step_planner) + # self.foot_step_planner.head_movement([1, 1, 0]) if self.imu_feedback_enabled and self.bez.sensors.imu_ready: [_, pitch, roll] = self.bez.sensors.get_imu() + # print(pitch," ", roll) self.stabilize_walk(pitch, roll) + # self.foot_step_planner.head_movement(target_goal.position) + + # self.bez.motor_control.configuration["head_yaw"] = self.ball_dx + # self.bez.motor_control.configuration["head_pitch"] = self.ball_dy + # self.bez.motor_control.configuration_offset["left_hip_pitch"] = 0.15 + # self.bez.motor_control.configuration_offset["right_hip_pitch"] = 0.15 + self.bez.motor_control.configuration["left_elbow"] = 1.57 + self.bez.motor_control.configuration["right_elbow"] = 1.57 + self.bez.motor_control.configuration["left_shoulder_roll"] = 0.1 + self.bez.motor_control.configuration["right_shoulder_roll"] = 0.1 + # self.bez.motor_control.configuration["head_pitch"] = 0.7 + # self.bez.motor_control.set_single_motor("head_yaw", 0.7) + # self.bez.motor_control.set_right_leg_target_angles( + # [0.031498273770675496, 0.13013599705387854, 0.8763616115800654, -1.4338038100846235, 0.5569438675976406, -0.09870240716415977] + # ) + # self.bez.motor_control.set_left_leg_target_angles( + # [-0.031396938877692564, 0.07407130663246327, 0.8245393702946127, -1.3833448587348056, 0.5583100126857369, -0.10550492155783667] + # ) self.bez.motor_control.set_motor() - self.func_step() - t = self.foot_step_planner.step(t) + self.t = self.foot_step_planner.step(self.t) # update joints in control # TODO investigate it has an effect but not sure how much also with step time # for joint in self.bez.motor_control.motor_names: @@ -221,8 +297,6 @@ def walk_loop(self, t: float): # if self.record_walking_metrics: # self.update_walking_metrics(t) - return t - @staticmethod def heading_error(theta_desired: float, theta_current: float) -> float: """ @@ -249,10 +323,23 @@ def heading_error(theta_desired: float, theta_current: float) -> float: def position_error(goal_loc: np.ndarray, curr_loc: np.ndarray = (0, 0)): return float(np.linalg.norm(goal_loc - curr_loc)) + def kick_ready(self): + self.t2 = 0 + self.kick_planner.setup_tasks() + + self.set_angles_from_placo(self.kick_planner) + + self.bez.motor_control.set_motor() + def ready(self): self.foot_step_planner.setup_tasks() - self.bez.motor_control.configuration = self.filter_joints() + self.set_angles_from_placo(self.foot_step_planner) + self.bez.motor_control.configuration["left_shoulder_roll"] = 0.1 + self.bez.motor_control.configuration["right_shoulder_roll"] = 0.1 + + self.bez.motor_control.set_single_motor("head_yaw", self.ball_dx) + self.bez.motor_control.set_single_motor("head_pitch", self.ball_dy) self.bez.motor_control.set_motor() def wait(self, step: int) -> None: @@ -262,14 +349,12 @@ def stabilize_walk(self, pitch: float, roll: float) -> None: error_pitch = self.walk_pid.walking_pitch_pid.update(pitch) self.bez.motor_control.set_leg_joint_3_target_angle(error_pitch) # TODO retune pass - error_roll = self.walk_pid.walking_pitch_pid.update(roll) # TODO retune + error_roll = self.walk_pid.walking_roll_pid.update(roll) # TODO retune self.bez.motor_control.set_leg_joint_2_target_angle(error_roll) - def filter_joints(self) -> List[int]: - joints = [0] * self.bez.motor_control.numb_of_motors + def set_angles_from_placo(self, planner) -> None: for joint in self.bez.motor_control.motor_names: - joints[self.bez.motor_control.motor_names.index(joint)] = self.foot_step_planner.robot.get_joint(joint) - return joints + self.bez.motor_control.configuration[joint] = planner.robot.get_joint(joint) def display_walking_metrics(self, show_targets: bool = False) -> None: fig, (ax_imu0, ax_imu1, ax_imu2) = plt.subplots(3, 1, sharex=True) diff --git a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py index e41bc7a95..5fd9e967e 100644 --- a/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py +++ b/soccer_control/soccer_pycontrol/src/soccer_pycontrol/walk_engine/walk_engine_ros/navigator_ros.py @@ -1,50 +1,89 @@ +import numpy as np import rospy from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Float32MultiArray + from soccer_pycontrol.model.model_ros.bez_ros import BezROS from soccer_pycontrol.walk_engine.foot_step_planner import FootStepPlanner from soccer_pycontrol.walk_engine.navigator import Navigator from soccer_pycontrol.walk_engine.stabilize import Stabilize -from soccer_common import PID +from soccer_common import PID, Transformation +from soccer_msgs.msg import BoundingBoxes class NavigatorRos(Navigator): - def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False): + def __init__(self, bez: BezROS, imu_feedback_enabled: bool = False, ball2: bool = False): + self.ball = None + self.ball2 = ball2 + self.ball_pixel = None self.imu_feedback_enabled = imu_feedback_enabled self.bez = bez - self.foot_step_planner = FootStepPlanner(self.bez.robot_model, self.bez.parameters, rospy.get_time) + self.foot_step_planner = FootStepPlanner(self.bez.robot_model, self.bez.parameters, rospy.get_time, debug=False, ball=self.ball2) # TODO publish local odomtry from foot step planner self.rate = rospy.Rate(1 / self.foot_step_planner.DT) self.func_step = self.rate.sleep self.walk_pid = Stabilize(self.bez.parameters) - self.max_vel = 0.09 + self.max_vel = 0.03 self.nav_x_pid = PID( - Kp=0.5, + Kp=0.1, Kd=0, Ki=0, setpoint=0, output_limits=(-self.max_vel, self.max_vel), ) self.nav_y_pid = PID( # TODO properly tune later - Kp=0.5, + Kp=0.1, Kd=0, Ki=0, setpoint=0, - output_limits=(-0.05, 0.05), + output_limits=(-self.max_vel, self.max_vel), ) # TODO could also mod if balance is decreasing self.nav_yaw_pid = PID( - Kp=0.2, + Kp= 0.01, Kd=0, Ki=0, setpoint=0, - output_limits=(-0.3, 0.3), + output_limits=(-0.1, 0.1), ) - + self.ball_dx = 0 + self.ball_dy = 0.7 self.error_tol = 0.05 # in m TODO add as a param and in the ros version self.position_subscriber = rospy.Subscriber(self.bez.ns + "goal", PoseStamped, self.goal_callback) self.goal = PoseStamped() + self.t = None + self.enable_walking = None + self.reset_walk() + self.sub_boundingbox = rospy.Subscriber("/robot1/ball", PoseStamped, self.box_callback) + self.sub_ball_pixel = rospy.Subscriber("/robot1/ball_pixel", Float32MultiArray, self.pixel_callback) + self.last_ball = [0, 0] + self.last_req = rospy.Time.from_sec(0) + self.ball_x_pid = PID( + Kp=0.05, + Kd=0, + Ki=0.03, + setpoint=0, + output_limits=(-1.57, 1.57), + ) + + self.ball_y_pid = PID( + Kp=-0.05, + Kd=0, + Ki=-0.03, + setpoint=2.4, + output_limits=(0.4, 1.3), + ) + def check_request_timeout(self, nsecs: int = 500000000): + return (rospy.Time.now() - self.last_req) < rospy.Duration(secs=1, nsecs=nsecs) + + def pixel_callback(self, data): + self.ball_pixel = data.data + + def box_callback(self, data): + self.last_req = rospy.Time.now() + self.ball = Transformation(pose=data.pose) def goal_callback(self, pose: PoseStamped) -> None: """ @@ -61,3 +100,66 @@ def goal_callback(self, pose: PoseStamped) -> None: def wait(self, steps: int): for i in range(steps): rospy.sleep(self.foot_step_planner.DT) + + def run(self, target_goal): + angles = np.linspace(-np.pi, np.pi) + ready = True + while not rospy.is_shutdown(): + # self.bez.motor_control.set_single_motor("head_pitch", 0.7) + # self.bez.motor_control.set_single_motor("head_yaw", 0.0) + # self.bez.motor_control.set_motor() + + if isinstance(target_goal, Transformation): + if self.check_request_timeout() and self.ball_pixel is not None: + ready = False + self.walk(self.ball, self.ball_pixel, True) + elif not ready: + self.ready() + self.bez.motor_control.set_single_motor("head_pitch", 0.7) + ready = True + elif isinstance(target_goal, + list): # [d_x: float = 0.0, d_y: float = 0.0, d_theta: float = 0.0, nb_steps: int = 10, t_goal: float = 10] + # if target_goal[:3] == [0.0,0.0,0]: + # if self.imu_feedback_enabled and self.bez.sensors.imu_ready: + # [_, pitch, roll] = self.bez.sensors.get_imu() + # print(pitch," ", roll) + # self.stabilize_walk(pitch, roll) + # + # + # self.bez.motor_control.configuration["head_yaw"] = self.ball_dx + # self.bez.motor_control.configuration["head_pitch"] = self.ball_dy + # self.bez.motor_control.configuration["left_elbow"] = 1.57 + # self.bez.motor_control.configuration["right_elbow"] = 1.57 + # self.bez.motor_control.configuration["left_shoulder_roll"] = 0.1 + # self.bez.motor_control.configuration["right_shoulder_roll"] = 0.1 + # + # self.bez.motor_control.set_motor() + # else: + self.walk_time(target_goal) + + + # print(f"Height rotation: {self.bez.sensors.get_height().orientation_euler}") + # print(f"Height position: {self.bez.sensors.get_height().position}") + + # self.walk(target_goal, False) + # self.walk(target_goal, True) + # if self.bez.sensors.imu_ready: + # [_, pitch, roll] = self.bez.sensors.get_imu() + # print(pitch, " ", roll) + # self.stabilize_walk(pitch, roll) + # self.bez.motor_control.set_motor() + # for j in angles: + # + # # print(f"POS: tf: {self.bez.sensors.get_height().position} gt: {self.bez.sensors.get_global_height().position}") + # print( + # f"Eul: tf: {self.bez.sensors.get_height().orientation_euler} gt: {self.bez.sensors.get_global_height().orientation_euler}") + # self.bez.motor_control.configuration["head_yaw"] = -1.57 + # self.bez.motor_control.configuration["head_pitch"] = 1.57 + # self.bez.motor_control.set_right_leg_target_angles([0,0,0,0,0,0]) + # self.bez.motor_control.set_left_leg_target_angles([0, 0, 0, 0, 0, 0]) + # set_right_leg_target_angles + # self.bez.motor_control.set_motor() + # if i % 1000 == 0: + # walk.reset_walk() + + self.func_step() diff --git a/soccer_control/soccer_pycontrol/test/test_ball_localization.py b/soccer_control/soccer_pycontrol/test/test_ball_localization.py new file mode 100644 index 000000000..3108e0cc4 --- /dev/null +++ b/soccer_control/soccer_pycontrol/test/test_ball_localization.py @@ -0,0 +1,51 @@ +import unittest +from os.path import expanduser + +import numpy as np +from soccer_object_detection.object_detect_node import ObjectDetectionNode + +from soccer_msgs.msg import BoundingBox, BoundingBoxes + +REAL_TIME = True + + +class TestBallLocalization(unittest.TestCase): + def test_ball_localization(self): + src_path = expanduser("~") + "/ros2_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) + + bbs_msg = BoundingBoxes() + bbs_msg.bounding_boxes.append(BoundingBox()) + bbs_msg.bounding_boxes[0].xmin = 289 + bbs_msg.bounding_boxes[0].ymin = 153 + bbs_msg.bounding_boxes[0].xmax = 333 + bbs_msg.bounding_boxes[0].ymax = 197 + bbs_msg.bounding_boxes[0].Class = "0" + bbs_msg.bounding_boxes[0].probability = 0.9100908 + bbs_msg.bounding_boxes[0].xbase = 311 + bbs_msg.bounding_boxes[0].ybase = 194 + + camera_z_height = 0.48848283290863037 + camera_orientation = [0.051347, 0.61632, 0.04393] + + ball_estim = [1.4448, 0.027018, 0.69474] + ball_actual = [0.98048, 0.056295, -0.2648] + + ball_found = False + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + ball_found = True + + if ball_found: + ball_pos = detect.get_ball_position(bbs_msg, camera_z_height, camera_orientation) + # ball_pos = Transformation(position=ball_pos_euler, euler=[0, 0, 0]) + print( + f"floor pos: {np.round(ball_pos.position, 5).tolist()} " f"ball: {ball_estim}", + flush=True, + ) + detected_ball_pos = np.round(ball_pos.position, 5).tolist() + self.assertAlmostEqual(detected_ball_pos[0], ball_estim[0], delta=0.001) + self.assertAlmostEqual(detected_ball_pos[1], ball_estim[1], delta=0.001) + self.assertAlmostEqual(detected_ball_pos[2], ball_estim[2], delta=0.001) diff --git a/soccer_control/soccer_pycontrol/test/test_perception.py b/soccer_control/soccer_pycontrol/test/test_perception.py index 5722a181f..7145aca39 100644 --- a/soccer_control/soccer_pycontrol/test/test_perception.py +++ b/soccer_control/soccer_pycontrol/test/test_perception.py @@ -1,12 +1,15 @@ import unittest + import matplotlib.pyplot as plt from soccer_pycontrol.model.bez import Bez # Import the Bez class -from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld # Import the PybulletWorld class +from soccer_pycontrol.pybullet_usage.pybullet_world import ( + PybulletWorld, # Import the PybulletWorld class +) + from soccer_common import Transformation class TestSensors(unittest.TestCase): - def setUp(self): """ Set up the PyBullet environment and the robot with sensors. @@ -17,7 +20,7 @@ def setUp(self): rate=200, ) - self.robot_model = "bez2" + self.robot_model = "assembly" self.bez = Bez(robot_model=self.robot_model, pose=Transformation()) def tearDown(self): @@ -38,9 +41,10 @@ def test_camera_image_capture(self): # Display the captured image plt.imshow(image) - plt.title('Captured Camera Image') - plt.axis('off') + plt.title("Captured Camera Image") + plt.axis("off") plt.show() + if __name__ == "__main__": - unittest.main() \ No newline at end of file + unittest.main() diff --git a/soccer_control/soccer_pycontrol/test/test_placo.py b/soccer_control/soccer_pycontrol/test/test_placo.py index 0c9197619..9310ae3f2 100644 --- a/soccer_control/soccer_pycontrol/test/test_placo.py +++ b/soccer_control/soccer_pycontrol/test/test_placo.py @@ -1,10 +1,20 @@ +import os import unittest +from os.path import expanduser +from random import uniform +import cv2 +import numpy as np +import pybullet as pb +import pytest +from soccer_object_detection.object_detect_node import ObjectDetectionNode from soccer_pycontrol.model.bez import Bez from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld from soccer_pycontrol.walk_engine.navigator import Navigator +from soccer_trajectories.trajectory_manager_sim import TrajectoryManagerSim from soccer_common import Transformation +from soccer_msgs.msg import BoundingBox, BoundingBoxes REAL_TIME = True @@ -16,74 +26,487 @@ def tearDown(self): del self.world def test_bez1(self): + src_path = expanduser("~") + "/ros2_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/yolov8s_detect_best.pt" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) + self.world = PybulletWorld( camera_yaw=90, real_time=REAL_TIME, rate=200, ) self.bez = Bez(robot_model="assembly", pose=Transformation()) - walk = Navigator(self.world, self.bez, imu_feedback_enabled=False) + tm = TrajectoryManagerSim(self.world, self.bez, "bez2_sim", "getupfront") + + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + walk = Navigator(self.world, self.bez, imu_feedback_enabled=False, ball=True) # walk.ready() - # self.bez.motor_control.set_motor() - # walk.wait(50) - target_goal = [1, 0, 0.0, 10, 500] - # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0]) + # walk.wait(100) + target_goal = [0.05, 0, 0.0, 10, 500] + # target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) print("STARTING WALK") - walk.walk(target_goal, display_metrics=False) - # walk.wait(1000) + ball_pos = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + kicked = False + ball_found = False + + ball_pixel = [0, 0] + for i in range(10000): + if i % 10 == 0: + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + # detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + dimg, bbs_msg = detect.get_model_output(img) + + ball_found = False + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + ball_found = True + + if ball_found: + ball_pos = detect.get_ball_position( + bbs_msg, self.bez.sensors.get_pose(link=2).position[2], self.bez.sensors.get_pose(link=2).orientation_euler + ) + # ball_pos = Transformation(position=ball_pos_euler, euler=[0, 0, 0]) + ball_pos_actual = self.bez.sensors.get_ball() + print( + f"bounding box: {bbs_msg}" + f"camera height {self.bez.sensors.get_pose(link=2).position[2]}" + f"camera orientation {self.bez.sensors.get_pose(link=2).orientation_euler}" + f"floor pos: {ball_pos.position} " + f"ball: {ball_pos_actual.position}", + flush=True, + ) + # ball_pos = self.bez.sensors.get_ball() + # for box in bbs_msg.bounding_boxes: + # if box.Class == "0": + # detect.camera.pose.position = [0, 0, self.bez.sensors.get_pose(link=2).position[2]] + # detect.camera.pose.orientation_euler = self.bez.sensors.get_pose(link=2).orientation_euler + # # detect.camera.pose = self.bez.sensors.get_pose(link=2) + # boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] + # ball_pixel = [(box.xmin + box.xmax) / 2.0, (box.ymin + box.ymax) / 2.0] + # # print(detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position) + # kicked = False + # ball_pos = self.bez.sensors.get_ball() + # print( + # f"floor pos: {detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position} ball: {self.bez.sensors.get_ball().position}", + # flush=True, + # ) + # # pos = [box.xbase, box.ybase] + # # # detect.camera.pose.position = self.bez.sensors.get_pose(link=2).position + # # detect.camera.pose.position = [0, 0, self.bez.sensors.get_pose(link=2).position[2]] + # # detect.camera.pose.orientation_euler = self.bez.sensors.get_pose(link=2).orientation_euler + # # floor_coordinate_robot = detect.camera.find_floor_coordinate(pos) + # # print(f"floor pos: {floor_coordinate_robot} ball: {self.bez.sensors.get_ball().position}") + # # + # # ball_pos = Transformation(position=floor_coordinate_robot) + # # temp = self.bez.sensors.get_pose().rotation_matrix @ self.bez.sensors.get_ball().position + self.bez.sensors.get_pose().position + # # print(f"pos2: {temp} ball: {self.bez.sensors.get_ball_global().position}") + # temp1 = detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position + # temp2 = self.bez.sensors.get_ball().position + # font = cv2.FONT_HERSHEY_DUPLEX + # color = (255, 255, 255) # red + # fontsize = 255 + # text = "test" + # position = (10, 10) + + # cv2.putText(dimg, text, position, font, fontsize, color=color) + if "DISPLAY" in os.environ: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) - # TODO fix - def test_bez2(self): + # if ball_found == False: + # continue + + if 0 < np.linalg.norm(ball_pos.position[:2]) < 0.2 and not kicked and ball_found: + walk.ready() + walk.wait(100) + tm.send_trajectory("rightkick") + kicked = True + # walk.kick_ready() + # walk.kick() + + walk.reset_walk() + + elif ball_found: + walk.walk(ball_pos_actual, ball_pixel, True) + # print(f"Height rotation: {self.bez.sensors.get_height().orientation_euler}", flush=True) + # print(f"Height position: {self.bez.sensors.get_height().position}", flush=True) + + # walk.walk(target_goal, display_metrics=False) + # if not walk.enable_walking: + # print("WALK ENABLED") + # x = uniform(-1, 1) # TODO own unit test for yaw + # y = uniform(-1, 1) + # theta = uniform(-3.14, 3.14) + # print(x, y, theta) + # target_goal = Transformation(position=[x, y, 0], euler=[theta, 0, 0]) + # walk.reset_walk() + self.world.step() + + def test_ball_localization(self): + # check test_camera and the assertion self.world = PybulletWorld( camera_yaw=90, real_time=REAL_TIME, rate=200, ) - self.bez = Bez(robot_model="bez2", pose=Transformation()) - walk = Navigator(self.world, self.bez) - walk.walk(d_x=0.03, t_goal=10) + self.bez = Bez(robot_model="assembly", pose=Transformation(), fixed_base=True) - def test_bez1_start_stop(self): + src_path = expanduser("~") + "/ros2_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/yolov8s_detect_best.pt" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) + + bbs_msg = BoundingBoxes() + bbs_msg.bounding_boxes.append(BoundingBox()) + bbs_msg.bounding_boxes[0].xmin = 289 + bbs_msg.bounding_boxes[0].ymin = 153 + bbs_msg.bounding_boxes[0].xmax = 333 + bbs_msg.bounding_boxes[0].ymax = 197 + bbs_msg.bounding_boxes[0].Class = "0" + bbs_msg.bounding_boxes[0].probability = 0.9100908 + bbs_msg.bounding_boxes[0].xbase = 311 + bbs_msg.bounding_boxes[0].ybase = 194 + + camera_z_height = 0.48848283290863037 + camera_orientation = [0.051347, 0.61632, 0.04393] + + ball_estim = [1.4448, 0.027018, 0.69474] + ball_actual = [0.98048, 0.056295, -0.2648] + + ball_found = False + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + ball_found = True + + if ball_found: + ball_pos = detect.get_ball_position(bbs_msg, camera_z_height, camera_orientation) + # ball_pos = Transformation(position=ball_pos_euler, euler=[0, 0, 0]) + print( + f"floor pos: {np.round(ball_pos.position, 5).tolist()} " f"ball: {ball_estim}", + flush=True, + ) + detected_ball_pos = np.round(ball_pos.position, 5).tolist() + self.assertAlmostEqual(detected_ball_pos[0], ball_estim[0], delta=0.001) + self.assertAlmostEqual(detected_ball_pos[1], ball_estim[1], delta=0.001) + self.assertAlmostEqual(detected_ball_pos[2], ball_estim[2], delta=0.001) + + # new test class that is independent of pybullet teardown etc + + # unit test for the walk function, passing different cases, can just do visual test + + # kick when close, in bez, might have to tell where the ball is, wont look down, + + # bez model and pubulllet load model need a way to control where the ball gets spawned + + # in navigator get rid of the fixed motor angles, use fixed base instead + + # @pytest.mark.parametrize("test_list", [1]) # test list + # @pytest.mark.parametrize("test transformation", "test2") + # @pytest.mark.parametrize("test time", "test3") + def test_walk_pose(self): self.world = PybulletWorld( camera_yaw=90, real_time=REAL_TIME, rate=200, ) - self.bez = Bez(robot_model="bez1", pose=Transformation()) - walk = Navigator(self.world, self.bez) - walk.walk(d_x=0.03, t_goal=5) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + + walk = Navigator( + self.world, self.bez, imu_feedback_enabled=False, ball=True + ) # note that ball=True if not foot_step_planner.head_movement fails + target_goal = Transformation(position=[0.5, 0, 0], euler=[0, 0, 0]) # half a square in front + + for i in range(10000): + + walk.walk(target_goal) + self.world.step() + + def test_walk_ball(self): # leave as a behaviour + src_path = expanduser("~") + "/catkin_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/yolov8s_detect_best.pt" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + + detect = ObjectDetectionNode(model_path) + + self.world = PybulletWorld( + camera_yaw=90, + real_time=REAL_TIME, + rate=200, + ) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + tm = TrajectoryManagerSim(self.world, self.bez, "bez2_sim", "getupfront") + + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + walk = Navigator(self.world, self.bez, imu_feedback_enabled=False, ball=True) + # walk.ready() + # walk.wait(100) + target_goal = [0.05, 0, 0.0, 10, 500] + # target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + print("STARTING WALK") + ball_pos = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + kicked = False + ball_found = False + + ball_pixel = [0, 0] + for i in range(10000): + if i % 10 == 0: + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + dimg, bbs_msg = detect.get_model_output(img) + + ball_found = False + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + ball_found = True + + if ball_found: + ball_pos = detect.get_ball_position( + bbs_msg, self.bez.sensors.get_pose(link=2).position[2], self.bez.sensors.get_pose(link=2).orientation_euler + ) + # ball_pos = Transformation(position=ball_pos_euler, euler=[0, 0, 0]) + ball_pos_actual = self.bez.sensors.get_ball() + print( + f"bounding box: {bbs_msg}" + f"camera height {self.bez.sensors.get_pose(link=2).position[2]}" + f"camera orientation {self.bez.sensors.get_pose(link=2).orientation_euler}" + f"floor pos: {ball_pos.position} " + f"ball: {ball_pos_actual.position}", + flush=True, + ) + + if "DISPLAY" in os.environ: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) + + if 0 < np.linalg.norm(ball_pos.position[:2]) < 0.2 and not kicked and ball_found: + walk.ready() + walk.wait(100) + tm.send_trajectory("rightkick") + kicked = True + + walk.reset_walk() + + elif ball_found: + walk.walk(ball_pos_actual, ball_pixel, True) + + self.world.step() + + def test_walk_time(self): + self.world = PybulletWorld( + camera_yaw=90, + real_time=REAL_TIME, + rate=200, + ) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + + walk = Navigator( + self.world, self.bez, imu_feedback_enabled=False, ball=True + ) # note that ball=True if not foot_step_planner.head_movement fails + target_goal = [0.0, 0.0, 0.0, 10, 10] + + for i in range(10000): + walk.walk(target_goal) + self.world.step() + + def test_camera(self): + src_path = expanduser("~") + "/ros2_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/yolov8s_detect_best.pt" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + detect = ObjectDetectionNode(model_path) + + cap = cv2.VideoCapture(4) + if not cap.isOpened(): + print("Cannot open camera") + exit() + + # self.world = PybulletWorld( + # camera_yaw=90, + # real_time=REAL_TIME, + # rate=200, + # ) + # self.bez = Bez(robot_model="assembly", pose=Transformation()) + # tm = TrajectoryManagerSim(self.world, self.bez, "bez2_sim", "getupfront") + + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + # walk = Navigator(self.world, self.bez, imu_feedback_enabled=False, ball=True) + # walk.ready() + # walk.wait(100) + target_goal = [0.05, 0, 0.0, 10, 500] + # target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + print("STARTING WALK") + ball_pos = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + kicked = False + while True: + # img = self.bez.sensors.get_camera_image() + ret, frame = cap.read() + if not ret: + print("Can't receive frame (stream end?). Exiting ...") + break + img = cv2.resize(frame, dsize=(640, 480)) + # detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + # dimg, bbs_msg = detect.get_model_output_v8(img) + dimg, bbs_msg = detect.get_model_output(img) + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + detect.camera.pose.position = [0, 0, 0.6] + # detect.camera.pose.orientation_euler = self.bez.sensors.get_pose(link=2).orientation_euler + detect.camera.pose.orientation_euler = [-0.029378, -0.11132, 0.063983] + # detect.camera.pose = self.bez.sensors.get_pose(link=2) + boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] + # print(detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position) + # kicked = False + # ball_pos = self.bez.sensors.get_ball() + print(f"floor pos: {detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position}", flush=True) + # print(detect.camera.pose.orientation_euler) + print() + + if "DISPLAY" in os.environ: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) + + # self.world.step() + + def test_bez1_walk(self): + + self.world = PybulletWorld( + camera_yaw=90, + real_time=REAL_TIME, + rate=200, + ) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + walk = Navigator(self.world, self.bez, imu_feedback_enabled=False) + walk.ready() walk.wait(100) - walk.walk(d_x=0.03, t_goal=5) - walk.wait(500) + target_goal = [0.08, 0, 0.0, 10, 500] + # target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + print("STARTING WALK") + for i in range(10000): + + # walk.walk(ball_pos, True) + walk.walk(target_goal, display_metrics=False) + # if not walk.enable_walking: + # print("WALK ENABLED") + # x = uniform(-1, 1) # TODO own unit test for yaw + # y = uniform(-1, 1) + # theta = uniform(-3.14, 3.14) + # print(x, y, theta) + # target_goal = Transformation(position=[x, y, 0], euler=[theta, 0, 0]) + # walk.reset_walk() + self.world.step() + walk.wait(10000) + + def test_bez1_auto(self): + + self.world = PybulletWorld( + camera_yaw=90, + real_time=REAL_TIME, + rate=200, + ) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + tm = TrajectoryManagerSim(self.world, self.bez, "bez2_sim", "getupfront") + + for i in range(100000): + y, p, r = self.bez.sensors.get_imu() + print(y, " ", p, " ", r) + + if p > 1.25: + print("getupfront") + tm.send_trajectory("getupfront") + elif p < -1.25: + print("getupback: ") + tm.send_trajectory("getupback") + elif r < -1.54 and -0.5 < p < -0.4: + tm.send_trajectory("getupsideleft") + elif r > 1.54 and -0.5 < p < -0.4: + tm.send_trajectory("getupsideright") + self.world.step() + + def test_bez1_kick(self): - def test_bez1_replan(self): self.world = PybulletWorld( camera_yaw=90, real_time=REAL_TIME, rate=200, ) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + walk = Navigator(self.world, self.bez, imu_feedback_enabled=False) + walk.kick_ready() + walk.wait(200) + # target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + print("STARTING WALK") + for i in range(10000): + walk.kick() + + self.world.step() + + def test_bez_motor_range_single(self): + self.world = PybulletWorld( + camera_yaw=90, + real_time=REAL_TIME, + rate=200, + ) + # self.bez = Bez(robot_model="assembly", pose=Transformation()) self.bez = Bez(robot_model="bez1", pose=Transformation()) - walk = Navigator(self.world, self.bez) - walk.foot_step_planner.setup_walk(d_x=0.03) - walk.pid.reset_imus() - t = 0 - while t < 15: # TODO needs end condition - [_, pitch, roll] = walk.bez.sensors.get_imu() - if t > 3: - walk.foot_step_planner.configure_planner(d_y=0.03) - if t > 6: - walk.foot_step_planner.configure_planner(d_x=0.0, d_theta=0.4) - if t > 9: - walk.foot_step_planner.configure_planner(d_x=0.03, d_y=0.03) - walk.foot_step_planner.plan_steps(t) - - walk.bez.motor_control.configuration = walk.filter_joints() - walk.stabilize_walk(pitch, roll) - - walk.bez.motor_control.set_motor() - walk.world.step() - - t = walk.foot_step_planner.step(t) + walk = Navigator(self.world, self.bez, imu_feedback_enabled=False) + walk.ready() + walk.wait(100) + + angles = np.linspace(-np.pi, np.pi) + + for j in angles: + # if i % 10 == 0: + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + + if "DISPLAY" in os.environ: + cv2.imshow("CVT Color2", img) + cv2.waitKey(1) + t = "shoulder_roll" + self.bez.motor_control.configuration["right_" + t] = j + self.bez.motor_control.configuration["left_" + t] = j + # self.bez.motor_control.configuration[self.bez.motor_control.get_motor_array_index("head_yaw")] = j + # x[self.bez.motor_control.get_motor_array_index("head_yaw")] = j + # x[self.bez.motor_control.motor_names.index("right_"+t)] = j + # x[self.bez.motor_control.motor_names.index("left_"+t)] = j + self.bez.motor_control.set_motor() + + self.world.wait_motor() + + for i in range(10000): + img = self.bez.sensors.get_camera_image() + img = cv2.resize(img, dsize=(640, 480)) + + if "DISPLAY" in os.environ: + cv2.imshow("CVT Color2", img) + cv2.waitKey(1) + self.world.step() + + def test_bez1_start_stop(self): + self.world = PybulletWorld( + camera_yaw=90, + real_time=REAL_TIME, + rate=200, + ) + self.bez = Bez(robot_model="assembly", pose=Transformation()) + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + walk = Navigator(self.world, self.bez, imu_feedback_enabled=False) + walk.ready() + walk.wait(100) + target_goal = [1, 0, 0, 10, 2] + print("STARTING WALK") + + for i in range(10000): + walk.walk(target_goal, display_metrics=False) + if i % 1000 == 0: + walk.reset_walk() + + self.world.step() def test_bez1_ready(self): self.world = PybulletWorld( diff --git a/soccer_control/soccer_pycontrol/test/test_pybullet.py b/soccer_control/soccer_pycontrol/test/test_pybullet.py index 8c29b47cf..407e7ae9a 100644 --- a/soccer_control/soccer_pycontrol/test/test_pybullet.py +++ b/soccer_control/soccer_pycontrol/test/test_pybullet.py @@ -1,5 +1,7 @@ import unittest +from os.path import expanduser +import cv2 import numpy as np import pybullet as pb import pytest @@ -60,24 +62,21 @@ def test_bez_motor_range(self): def test_bez_motor_range_single(self): self.world = PybulletWorld(path="", camera_yaw=45, real_time=REAL_TIME, rate=500) - self.bez = Bez(robot_model="bez1", fixed_base=True, pose=Transformation()) - # self.bez = Bez(robot_model="assembly", fixed_base=True, pose=Transformation()) + # self.bez = Bez(robot_model="bez1", fixed_base=True, pose=Transformation()) + self.bez = Bez(robot_model="assembly", fixed_base=True, pose=Transformation()) self.world.wait(50) angles = np.linspace(-np.pi, np.pi) for j in angles: - x = [0.0] * self.bez.motor_control.numb_of_motors - t = "elbow" - x[self.bez.motor_control.motor_names.index("head_pitch")] = j + # x = [0.0] * self.bez.motor_control.numb_of_motors + t = "shoulder_roll" + # x[self.bez.motor_control.motor_names.index("head_pitch")] = j + self.bez.motor_control.configuration["right_" + t] = j + self.bez.motor_control.configuration["left_" + t] = j # x[self.bez.motor_control.motor_names.index("right_"+t)] = j # x[self.bez.motor_control.motor_names.index("left_"+t)] = j - pb.setJointMotorControlArray( - bodyIndex=self.bez.model.body, - controlMode=pb.POSITION_CONTROL, - jointIndices=list(range(0, self.bez.motor_control.numb_of_motors, 1)), - targetPositions=x, - ) + self.bez.motor_control.set_motor() self.world.wait_motor() self.world.wait(100) diff --git a/soccer_control/soccer_pycontrol/test/test_walk_ros.py b/soccer_control/soccer_pycontrol/test/test_walk_ros.py index 8e3d3574d..214e75941 100644 --- a/soccer_control/soccer_pycontrol/test/test_walk_ros.py +++ b/soccer_control/soccer_pycontrol/test/test_walk_ros.py @@ -1,9 +1,13 @@ import os import time import unittest +from os.path import expanduser +import cv2 import rospy from geometry_msgs.msg import PoseStamped + +from soccer_object_detection.object_detect_node import ObjectDetectionNode from soccer_pycontrol.model.model_ros.bez_ros import BezROS from soccer_pycontrol.walk_engine.walk_engine_ros.navigator_ros import NavigatorRos @@ -16,26 +20,169 @@ class TestPybullet(unittest.TestCase): @unittest.skipIf("DISPLAY" not in os.environ, "only local") def test_walk_ros_local(self): robot_ns = os.environ["ROS_NAMESPACE"] - os.system( - f"/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill {robot_ns}/soccer_strategy {robot_ns}/soccer_pycontrol {robot_ns}/soccer_trajectories'" - ) + # os.system( + # f"/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill {robot_ns}/soccer_strategy {robot_ns}/soccer_pycontrol {robot_ns}/soccer_trajectories'" + # ) rospy.init_node("soccer_control") bez = BezROS() - walker = NavigatorRos(bez) + walker = NavigatorRos(bez, imu_feedback_enabled= True, ball2=True) # walker.wait(50) - # walker.ready() + walker.ready() + # bez.motor_control.set_single_motor("head_pitch", 0.7) # bez.motor_control.set_motor() + walker.wait(50) + # walker.goal_callback(PoseStamped()) + # walker.walk(d_x=0.04, t_goal=10) + # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0]) + # walker.walk(target_goal) + # target_goal = [0.03 , 0.0, 0, 10, 500] + target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + # walker.walk(target_goal) + walker.run(target_goal) + + walker.wait(100) + + def test_ready_ros_local(self): + robot_ns = os.environ["ROS_NAMESPACE"] + # os.system( + # f"/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill {robot_ns}/soccer_strategy {robot_ns}/soccer_pycontrol {robot_ns}/soccer_trajectories'" + # ) + + rospy.init_node("soccer_control") + + bez = BezROS() + walker = NavigatorRos(bez, imu_feedback_enabled= True, ball2=True) # walker.wait(50) + walker.ready() + walker.wait(50) # walker.goal_callback(PoseStamped()) # walker.walk(d_x=0.04, t_goal=10) # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0]) # walker.walk(target_goal) - target_goal = [0.04, 0, 0, 10, 500] - walker.walk(target_goal) + # target_goal = [0.03, 0.0, 0, 10, 500] + # target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + # # walker.walk(target_goal) + # walker.run(target_goal) walker.wait(100) + def test_camera2(self): + robot_ns = os.environ["ROS_NAMESPACE"] + + + rospy.init_node("soccer_control") + bez = BezROS() + + + + # self.world = PybulletWorld( + # camera_yaw=90, + # real_time=REAL_TIME, + # rate=200, + # ) + # self.bez = Bez(robot_model="assembly", pose=Transformation()) + # tm = TrajectoryManagerSim(self.world, self.bez, "bez2_sim", "getupfront") + + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + # walk = Navigator(self.world, self.bez, imu_feedback_enabled=False, ball=True) + # walk.ready() + # walk.wait(100) + bez.motor_control.set_single_motor("head_yaw", 0) + rospy.sleep(0.5) + bez.motor_control.set_motor() + rospy.sleep(0.5) + rospy.sleep(1) + print(f"Height rotation: {bez.sensors.get_height().orientation_euler}") + print(f"Height position: {bez.sensors.get_height().position}") + bez.motor_control.set_single_motor("head_yaw", 1) + rospy.sleep(0.5) + bez.motor_control.set_motor() + rospy.sleep(0.5) + print(f"Height rotation2: {bez.sensors.get_height().orientation_euler}") + print(f"Height position2: {bez.sensors.get_height().position}") + + def test_camera(self): + robot_ns = os.environ["ROS_NAMESPACE"] + # os.system( + # f"/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill {robot_ns}/soccer_strategy {robot_ns}/soccer_pycontrol {robot_ns}/soccer_trajectories'" + # ) + + rospy.init_node("soccer_control") + bez = BezROS() + + walker = NavigatorRos(bez, imu_feedback_enabled=True, ball2=True) + src_path = expanduser("~") + "/ros2_ws/src/soccerbot/soccer_perception/" + model_path = src_path + "soccer_object_detection/models/yolov8s_detect_best.pt" + model_path = src_path + "soccer_object_detection/models/half_5.pt" + detect = ObjectDetectionNode(model_path) + + # target_goal = Transformation(position=[1, 0, 0], euler=[0, 0, 0]) + # walker.walk(target_goal) + cap = cv2.VideoCapture(4) + if not cap.isOpened(): + print("Cannot open camera") + exit() + + # self.world = PybulletWorld( + # camera_yaw=90, + # real_time=REAL_TIME, + # rate=200, + # ) + # self.bez = Bez(robot_model="assembly", pose=Transformation()) + # tm = TrajectoryManagerSim(self.world, self.bez, "bez2_sim", "getupfront") + + # self.bez = Bez(robot_model="bez1", pose=Transformation()) + # walk = Navigator(self.world, self.bez, imu_feedback_enabled=False, ball=True) + # walk.ready() + # walk.wait(100) + target_goal = [0.05, 0, 0.0, 10, 500] + # target_goal = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + print("STARTING WALK") + + ball_pos = Transformation(position=[0, 0, 0], euler=[0, 0, 0]) + kicked = False + while not rospy.is_shutdown(): + # img = self.bez.sensors.get_camera_image() + # bez.motor_control.set_single_motor("head_pitch", 0.7) + walker.ready() + bez.motor_control.set_motor() + ret, frame = cap.read() + if not ret: + print("Can't receive frame (stream end?). Exiting ...") + break + img = cv2.resize(frame, dsize=(640, 480)) + # detect.camera.pose.orientation_euler = [0, np.pi / 8, 0] + # dimg, bbs_msg = detect.get_model_output_v8(img) + dimg, bbs_msg = detect.get_model_output(img) + for box in bbs_msg.bounding_boxes: + if box.Class == "0": + detect.camera.pose.position = [0, 0, 0.6] + detect.camera.pose.position = [0, 0,bez.sensors.get_height().position[2]] + # detect.camera.pose.orientation_euler = self.bez.sensors.get_pose(link=2).orientation_euler + detect.camera.pose.orientation_euler = [-0.029378, -0.11132, 0.063983] + detect.camera.pose.orientation_euler = bez.sensors.get_height().orientation_euler + # detect.camera.pose = self.bez.sensors.get_pose(link=2) + boundingBoxes = [[box.xmin, box.ymin], [box.xmax, box.ymax]] + # print(detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position) + # kicked = False + ball_pos = detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes) + print(f"floor pos1: {detect.camera.calculate_ball_from_bounding_boxes(boundingBoxes).position}", flush=True) + + pos = [box.xbase, box.ybase] + floor_coordinate_robot = detect.camera.find_floor_coordinate(pos) + + print(f"floor pos2: {floor_coordinate_robot} ", flush=True) + + # print(detect.camera.pose.orientation_euler) + print(f"Height rotation: {bez.sensors.get_height().orientation_euler}", flush=True) + print(f"Height position: {bez.sensors.get_height().position}", flush=True) + + if "DISPLAY" in os.environ: + cv2.imshow("CVT Color2", dimg) + cv2.waitKey(1) + + # self.world.step() @unittest.skipIf("DISPLAY" not in os.environ, "only local") def test_walk_ros_webots(self): @@ -48,8 +195,8 @@ def test_walk_ros_webots(self): bez = BezROS(ns=ns) tmp = rospy.Publisher(ns + "reset_robot", PoseStamped) # tmp = rospy.Publisher(ns + "reset_robot", PoseStamped) - walker = NavigatorRos(bez) - walker.bez.motor_control.reset_target_angles() + walker = NavigatorRos(bez, ball2=True) + walker.bez.motor_control.configuration.reset() walker.bez.motor_control.set_motor() walker.wait(50) tmp.publish(PoseStamped()) @@ -61,9 +208,9 @@ def test_walk_ros_webots(self): start = time.time() # target_goal = Transformation(position=[1, 0, 0], euler=[0,0,0]) # walker.walk(target_goal) - walker.walk(bez.sensors.get_ball(), True) + walker.run(bez.sensors.get_ball()) # target_goal = [0.0, 0, 0, 10, 500] # walker.walk(target_goal) print("cm/s: ", 100 / (time.time() - start)) # walker.bez.sensors.get_ball(rospy.Time.now()) - walker.wait(100) + walker.wait(1000000) diff --git a/soccer_control/soccer_trajectories/CMakeLists.txt b/soccer_control/soccer_trajectories/CMakeLists.txt index c6a9e84fe..ae550bb1d 100644 --- a/soccer_control/soccer_trajectories/CMakeLists.txt +++ b/soccer_control/soccer_trajectories/CMakeLists.txt @@ -4,15 +4,15 @@ project(soccer_trajectories) add_compile_options(-std=c++11) find_package( - catkin + ros2 REQUIRED COMPONENTS dynamic_reconfigure tf2_ros rospy sensor_msgs rosgraph_msgs std_msgs ) -catkin_python_setup() +ros2_python_setup() -catkin_package( +ros2_package( CATKIN_DEPENDS dynamic_reconfigure tf2_ros @@ -22,4 +22,4 @@ catkin_package( std_msgs ) -include_directories(${catkin_INCLUDE_DIRS}) +include_directories(${ros2_INCLUDE_DIRS}) diff --git a/soccer_control/soccer_trajectories/package.xml b/soccer_control/soccer_trajectories/package.xml index 908c4826c..0364b8646 100644 --- a/soccer_control/soccer_trajectories/package.xml +++ b/soccer_control/soccer_trajectories/package.xml @@ -8,7 +8,7 @@ BSD - catkin + ros2 dynamic_reconfigure ros_control python3-scipy diff --git a/soccer_control/soccer_trajectories/setup.py b/soccer_control/soccer_trajectories/setup.py index 3b751cd21..a22091d2a 100644 --- a/soccer_control/soccer_trajectories/setup.py +++ b/soccer_control/soccer_trajectories/setup.py @@ -1,6 +1,6 @@ ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD -from catkin_pkg.python_setup import generate_distutils_setup +from ros2_pkg.python_setup import generate_distutils_setup from setuptools import setup # fetch values from package.xml diff --git a/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py b/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py deleted file mode 100644 index 914f26ade..000000000 --- a/soccer_control/soccer_trajectories/src/soccer_trajectories/pybullet_setup.py +++ /dev/null @@ -1,87 +0,0 @@ -import time -from os.path import expanduser -from typing import List - -import pybullet as pb -from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld - -from soccer_common import Transformation - - -class PybulletSetup: - """ - Sets up pybullet simulation for basic usage - """ - - # TODO update with the modified for pycontrol - def __init__( - self, pose: Transformation = Transformation(), robot_model: str = "bez1", real_time=False, rate: int = 75, display=True, camera_yaw=90 - ): - """ - Initialize the Navigator - - :param display: Whether or not to show the pybullet visualization, turned off for quick unit tests - :param useCalibration: Whether or not to use movement calibration files located in config/robot_model.yaml, which adjusts the calibration to the movement given - """ - self.rate = rate - self.display = display - self.real_time = real_time - - self.ramp = PybulletWorld( - "plane.urdf", (0, 0, 0), (0, 0, 0), lateral_friction=0.9, spinning_friction=0.9, rolling_friction=0.0, camera_yaw=camera_yaw - ) - home = expanduser("~") - self.body = pb.loadURDF( - home + f"/catkin_ws/src/soccerbot/soccer_description/{robot_model}_description/urdf/{robot_model}.urdf", - useFixedBase=False, - flags=pb.URDF_USE_INERTIA_FROM_FILE | (pb.URDF_MERGE_FIXED_LINKS if False else 0), - basePosition=pose.position, - baseOrientation=pose.quaternion, - ) - - self.motor_names = self.find_motor_names() - - self.numb_of_motors = len(self.motor_names) - self.max_forces = [] - - for i in range(0, self.numb_of_motors): - self.max_forces.append(pb.getJointInfo(self.body, i)[10] or 6) # TODO why is this acting so weird - - def find_motor_names(self) -> List[str]: - - names = [] - for i in range(pb.getNumJoints(self.body)): - joint_info = pb.getJointInfo(self.body, i) - if joint_info[2] == pb.JOINT_REVOLUTE: - names.append(joint_info[1].decode("utf-8")) - - return names - - def wait(self, steps) -> None: - """ - Make the robot wait for a few steps - - :param steps: Defined by Navigator.PYBULLET_STEP, which is usually 0.01 - """ - for i in range(steps): - self.step() - - def step(self) -> None: - if self.real_time: - time.sleep(1 / self.rate) - pb.stepSimulation() - - def motor_control(self, target: list) -> None: - pb.setJointMotorControlArray( - bodyIndex=self.body, - controlMode=pb.POSITION_CONTROL, - jointIndices=list(range(0, self.numb_of_motors, 1)), - targetPositions=target, - forces=self.max_forces, - ) - - -if __name__ == "__main__": - p = PybulletSetup(robot_model="bez2") - p.wait(1000) - p.ramp.close() diff --git a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager.py b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager.py index 225276c93..00e0b8bbf 100755 --- a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager.py +++ b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager.py @@ -1,5 +1,7 @@ #!/usr/bin/env python3 +import os from abc import ABC, abstractmethod +from os.path import expanduser from sensor_msgs.msg import JointState from soccer_trajectories.trajectory import Trajectory @@ -10,12 +12,14 @@ class TrajectoryManager(ABC): Interfaces with trajectory and sends to pybullet """ - def __init__(self, trajectory_path: str, mirror: bool = False): + def __init__(self, robot_model: str, traj_name: str, mirror: bool = False): + trajectory_path = expanduser("~") + f"/ros2_ws/src/soccerbot/soccer_control/soccer_trajectories/trajectories/{robot_model}/" + self.trajectory_path = trajectory_path - self.trajectory = Trajectory(trajectory_path, mirror=mirror) + self.trajectory = Trajectory(trajectory_path + traj_name + ".csv", mirror=mirror) - def process_trajectory(self, path: str, mirror: bool): - self.trajectory.trajectory_path = path + def process_trajectory(self, traj_name: str, mirror: bool): + self.trajectory.trajectory_path = self.trajectory_path + traj_name + ".csv" self.trajectory.mirror = mirror self.trajectory.reset() @@ -26,7 +30,7 @@ def read_joint_state(self) -> JointState: pass @abstractmethod - def send_trajectory(self) -> None: + def send_trajectory(self, traj_name: str) -> None: pass @abstractmethod diff --git a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_ros.py b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_ros.py index faff51ec6..9b9030d6c 100755 --- a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_ros.py +++ b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_ros.py @@ -19,7 +19,7 @@ class TrajectoryManagerRos(TrajectoryManager): Interfaces with trajectory and manages interaction with ROS """ - def __init__(self): + def __init__(self, robot_model: str, trajectory_name: str): use_sim_time_prefix = "_sim" if rospy.get_param("use_sim_time", "false") == "true" else "" # TODO fix path = ( @@ -29,11 +29,11 @@ def __init__(self): + use_sim_time_prefix ) # TODO fix later - super(TrajectoryManagerRos, self).__init__(path) + super(TrajectoryManagerRos, self).__init__(robot_model, trajectory_name) self.terminate = False - - self.rate = rospy.Rate(100) + self.period = 100 + self.rate = rospy.Rate(self.period) self.command_subscriber = rospy.Subscriber("command", FixedTrajectoryCommand, self.command_callback, queue_size=1) self.robot_state_subscriber = rospy.Subscriber("state", RobotState, self.robot_state_callback, queue_size=1) @@ -84,14 +84,18 @@ def send_trajectory(self, real_time: bool = True) -> None: t: float = 0 while not rospy.is_shutdown() and t <= self.trajectory.max_time and not self.terminate: try: + if t % 0.5: + print(1) self.send_joint_msg(t) except ROSException as ex: print(ex) exit(0) - t += 0.01 - if int(t + 0.01) != int(t): + t += 1.0 / self.period + if int(t + (1.0 / self.period)) != int(t): print(f"Trajectory at t={t}") - + # t += 0.01 + # if int(t + 0.01) != int(t): + # print(f"Trajectory at t={t}") if real_time: self.rate.sleep() diff --git a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py index d42547e16..8c4ee50b2 100755 --- a/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py +++ b/soccer_control/soccer_trajectories/src/soccer_trajectories/trajectory_manager_sim.py @@ -2,7 +2,8 @@ import time from sensor_msgs.msg import JointState -from soccer_trajectories.pybullet_setup import PybulletSetup +from soccer_pycontrol.model.bez import Bez +from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld from soccer_trajectories.trajectory_manager import TrajectoryManager from soccer_common import Transformation @@ -13,32 +14,23 @@ class TrajectoryManagerSim(TrajectoryManager): Interfaces with trajectory and sends to pybullet """ - def __init__( - self, - trajectory_path: str, - pose: Transformation = Transformation(), - robot_model: str = "bez1", - real_time=False, - mirror: bool = False, - camera_yaw=90, - ): - super(TrajectoryManagerSim, self).__init__(trajectory_path, mirror) - - self.sim = PybulletSetup(robot_model=robot_model, real_time=real_time, pose=pose, camera_yaw=camera_yaw) + def __init__(self, world: PybulletWorld, bez: Bez, robot_model: str, traj_name: str, mirror: bool = False): + super(TrajectoryManagerSim, self).__init__(robot_model, traj_name, mirror) + self.world = world + self.bez = bez def read_joint_state(self) -> JointState: - return JointState(name=self.sim.motor_names, position=[0.0] * self.sim.numb_of_motors) + return JointState(name=list(self.bez.motor_control.motor_names.keys()), position=[0.0] * self.bez.motor_control.numb_of_motors) def send_joint_msg(self, timestamp: float) -> None: - states = [0.0] * self.sim.numb_of_motors joints, angles = self.trajectory.create_joint_states(timestamp) for i, joint in enumerate(joints): - states[self.sim.motor_names.index(joint)] = angles[i] + self.bez.motor_control.configuration[joint] = angles[i] - self.sim.motor_control(states) + self.bez.motor_control.set_motor() - def send_trajectory(self, mirror: bool = False) -> None: - self.process_trajectory(self.trajectory_path, mirror) + def send_trajectory(self, traj_name: str, mirror: bool = False) -> None: + self.process_trajectory(traj_name, mirror) t: float = 0 while t <= self.trajectory.max_time: @@ -47,11 +39,11 @@ def send_trajectory(self, mirror: bool = False) -> None: except Exception as ex: print(ex) exit(0) - t += 1 / self.sim.rate + t += 1 / self.world.rate print("time", t) - self.sim.step() + self.world.step() # time.sleep(0.01) # self.sim.ramp.close() # self.trajectory.reset() diff --git a/soccer_control/soccer_trajectories/test/test_trajectory.py b/soccer_control/soccer_trajectories/test/test_trajectory.py index 29633df5b..53023d33f 100644 --- a/soccer_control/soccer_trajectories/test/test_trajectory.py +++ b/soccer_control/soccer_trajectories/test/test_trajectory.py @@ -3,8 +3,11 @@ from unittest.mock import MagicMock import pytest -import rospy -from sensor_msgs.msg import JointState + +# import rospy +# from sensor_msgs.msg import JointState +from soccer_pycontrol.model.bez import Bez +from soccer_pycontrol.pybullet_usage.pybullet_world import PybulletWorld from soccer_trajectories.trajectory import Trajectory from soccer_trajectories.trajectory_manager_ros import TrajectoryManagerRos from soccer_trajectories.trajectory_manager_sim import TrajectoryManagerSim @@ -52,7 +55,7 @@ def test_trajectory(self): self.assertEqual(angles, [0.0, 0.0, 0.0, 0.0, 0.564, 0.564, -1.176, -1.176, 0.613, 0.613, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) -@pytest.mark.parametrize("trajectory_name", ["getupfront"]) # , "getupback_full", "rightkick", "getupfront"]) +@pytest.mark.parametrize("trajectory_name", ["getupsideleft"]) # getupside rightkick " getupback getupfront @pytest.mark.parametrize("robot_model", ["assembly"]) @pytest.mark.parametrize("real_time", [True]) def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool): @@ -61,22 +64,31 @@ def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool) :return: None """ # TODO update with pybullet updates - if trajectory_name == "getupfront": + camera = 0 + if "getupfront" in trajectory_name: pose = Transformation(position=[0, 0, 0.070], euler=[0, 1.57, 0]) camera = 0 elif "getupback" in trajectory_name: pose = Transformation(position=[0, 0, 0.070], euler=[0, -1.57, 0]) camera = 0 + elif "getupside" in trajectory_name: + pose = Transformation(position=[0, 0, 0.13], euler=[0, 0, -1.57]) + camera = 0 else: - camera = 90 + camera = 45 pose = Transformation(position=[0, 0, 0.45], quaternion=[0.0, 0.0, 0.0, 1]) - print(os.path.join(os.path.dirname(__file__), "../trajectories/bez2/" + trajectory_name + ".csv")) - tm = TrajectoryManagerSim( - os.path.join(os.path.dirname(__file__), "../trajectories/bez2/" + trajectory_name + ".csv"), pose, robot_model, real_time, camera_yaw=camera + print(os.path.join(os.path.dirname(__file__), "../trajectories/bez2_sim/" + trajectory_name + ".csv")) + world = PybulletWorld( + camera_yaw=camera, + real_time=real_time, + rate=200, ) - tm.send_trajectory() - tm.sim.wait(100) - tm.sim.ramp.close() + bez = Bez(robot_model=robot_model, pose=pose) + + tm = TrajectoryManagerSim(world, bez, "bez2_sim", trajectory_name) + tm.send_trajectory(trajectory_name) + tm.world.wait(10000) + tm.world.close() # TODO add more testing from pybullet so like the height will reach a threshold and it doesnt fall over for # some time also maybe split it per trajectory type @@ -84,17 +96,17 @@ def test_trajectory_sim(trajectory_name: str, robot_model: str, real_time: bool) def run_real_trajectory(robot_model: str, trajectory_name: str, real_time: bool): # TODO clean up rospy.init_node("test") - os.system( - "/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill /robot1/soccer_strategy " - "/robot1/soccer_pycontrol /robot1/soccer_trajectories'" - ) + # os.system( + # "/bin/bash -c 'source /opt/ros/noetic/setup.bash && rosnode kill /robot1/soccer_strategy " + # "/robot1/soccer_pycontrol /robot1/soccer_trajectories'" + # ) file_path = os.path.dirname(os.path.abspath(__file__)) config_path = f"{file_path}/../../../{robot_model}_description/config/motor_mapping.yaml" set_rosparam_from_yaml_file(param_path=config_path) rospy.set_param("robot_model", robot_model) - c = TrajectoryManagerRos() + c = TrajectoryManagerRos(robot_model, trajectory_name) rospy.init_node("test") msg = FixedTrajectoryCommand() msg.trajectory_name = trajectory_name @@ -112,7 +124,7 @@ def run_real_trajectory(robot_model: str, trajectory_name: str, real_time: bool) @pytest.mark.parametrize("robot_model", ["bez2"]) -@pytest.mark.parametrize("trajectory_name", ["fix_angle_test"]) # getupback_full +@pytest.mark.parametrize("trajectory_name", ["rightkick"]) # fix_angle_test rightkick_2 getupback_old @pytest.mark.parametrize("real_time", [True]) @unittest.skipIf("DISPLAY" not in os.environ, "only local") def test_traj_ros(robot_model: str, trajectory_name: str, real_time: bool): diff --git a/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv b/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv index 7dedc30e9..a6a8b8364 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez1_sim/rightkick.csv @@ -15,4 +15,8 @@ right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,0.5,0,0,0.2 right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2,-0.1 right_shoulder_pitch,0,0,0,0,1,1,-1,-1,0 right_elbow,2,2,2,2,2,2,2,2,2 +right_shoulder_roll,0,0,0,0,0,0,0,0,0 +left_shoulder_roll,0,0,0,0,0,0,0,0,0 +head_yaw,0,0,0,0,0,0,0,0,0 +head_pitch,0,0,0,0,0,0,0,0,0 comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance,stance diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_full.csv# b/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_full.csv# deleted file mode 100644 index f77f16be0..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_full.csv# +++ /dev/null @@ -1 +0,0 @@ -,nam,nam-xps15,19.07.2024 04:03,file:///home/nam/.config/libreoffice/4; diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_left.csv# b/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_left.csv# deleted file mode 100644 index 42cfcfb30..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupback_left.csv# +++ /dev/null @@ -1 +0,0 @@ -,nam,nam-xps15,19.07.2024 17:46,file:///home/nam/.config/libreoffice/4; diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupfront.csv# b/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupfront.csv# deleted file mode 100644 index ce2957834..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/.~lock.getupfront.csv# +++ /dev/null @@ -1 +0,0 @@ -,nam,nam-xps15,19.07.2024 12:47,file:///home/nam/.config/libreoffice/4; diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/fix_angle_test.csv b/soccer_control/soccer_trajectories/trajectories/bez2/fix_angle_test.csv deleted file mode 100644 index 1d7a9604a..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/fix_angle_test.csv +++ /dev/null @@ -1,21 +0,0 @@ -time,0,0.5,1.25,2,2.75,3.5,4.25 -right_hip_yaw,0,0,0,0,0,0,0 -left_hip_yaw,0,0,0,0,0,0,0 -right_hip_roll,0,0,0,0,0,0,0 -left_hip_roll,0,0,0,0,0,0,0 -right_hip_pitch,0,0,0,0,0,0,0 -left_hip_pitch,0,0,0,0,0,0,0 -right_knee,0,0,0,0,0,0,0 -left_knee,0,0,0,0,0,0,0 -right_ankle_pitch,0,0,0,0,0,0,0 -left_ankle_pitch,0,0,0,0,0,0,0 -right_ankle_roll,0,0,0,0,0,0,0 -left_ankle_roll,0,0,0,0,0,0,0 -right_shoulder_pitch,0,3.14,3.14,3.14,3.14,3.14,3.14 -left_shoulder_pitch,0,0,0,0,0,0,0 -right_shoulder_roll,0,0,1.5,0,1.5,0,1.5 -left_shoulder_roll,0,0,0,0,0,0,0 -head_yaw,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,0 -right_elbow,0,0,0,0,0,0,0 -left_elbow,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupback.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupback.csv new file mode 100644 index 000000000..77ba13ae4 --- /dev/null +++ b/soccer_control/soccer_trajectories/trajectories/bez2/getupback.csv @@ -0,0 +1,21 @@ +time,0,0.5,1,1.5,2.25,2.5,2,2.75,3 +right_hip_yaw,0,0,0,0,0,0,0,0,0 +left_hip_yaw,0,0,0,0,0,0,0,0,0 +right_hip_roll,0,0,0,0,0,0,0,0,0 +left_hip_roll,0,0,0,0,0,0,0,0,0 +right_hip_pitch,0,1.3,0.2,-0.4,-0.4,-0.8,-0.4,0.4,0.4 +left_hip_pitch,0,1.3,0.2,-0.4,-0.4,-0.8,-0.4,0.4,0.4 +right_knee,0,-2,-2,-1.8,-1.8,-1.8,-1.8,-1.8,-0.8 +left_knee,0,-2,-2,-1.8,-1.8,-1.8,-1.8,-1.8,-0.8 +right_ankle_pitch,0,0.6,1.5,1.2,1,1.5,1,1.5,0.4 +left_ankle_pitch,0,0.6,1.5,1.2,1,1.5,1,1.5,0.4 +right_ankle_roll,0,0,0,0,0,0,0,0,0 +left_ankle_roll,0,0,0,0,0,0,0,0,0 +right_shoulder_pitch,0,-0.5,-1.6,-2.2,-1,-1,-2,-1,0 +left_shoulder_pitch,0,-0.5,-1.6,-2.2,-1,-1,-2,-1,0 +right_elbow,0,0.7,1.5,1.8,0,0,1.2,0,0 +left_elbow,0,0.7,1.5,1.8,0,0,1.2,0,0 +head_yaw,0,0,0,0,0,0,0,0,0 +head_pitch,0,0,0,0,0,0,0,0,0 +right_shoulder_roll,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_full.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupback_full.csv deleted file mode 100644 index a17e94b63..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_full.csv +++ /dev/null @@ -1,21 +0,0 @@ -time,0,0.2,2,2.1,3,3.5,4.5,5.5,6.5,7.5,11.5,12,12.5,13,13.5,14,15.5,16 -head_yaw,0,-2,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3,-1.3 -head_pitch,0,1.2,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6,0.6 -left_shoulder_pitch,0,0,-1.8,-3,-3,-3,0,0,3.2,3.2,2,1.3,1.8,1,0.9,0.9,0,0 -left_shoulder_roll,0,0,0.3,0.3,0.3,0.3,0.3,1.6,1.6,0,0,0,0,0,0,0,0,0 -left_elbow,0,0,1.3,0,0,0,0,0,0,0,2,1.3,0,0,0,0,0,0 -left_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_pitch,0,0,0,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,2.3,2.3,2.3,1,1,1,1 -left_knee,0,0,-2.5,0,-1.2,-1.2,-1.2,-1.2,-1.2,-1.2,-1.3,-1.8,-2.1,-3,-3,-3,-1.7,-1.7 -left_ankle_pitch,0,0,1,0,0.5,0.5,0.5,0.5,0.5,0.5,1.5,1.5,1.5,1.5,1.5,2,0.9,0.9 -left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_shoulder_pitch,0,0,-1.8,-3,-3,-3,0,0,3.2,3.2,2,1.3,1.8,1,0.9,0.9,0,0 -right_shoulder_roll,0,0,0.3,0.3,0.3,0.3,0.3,1.6,1.6,0,0,0,0,0,0,0,0,0 -right_elbow,0,0,1.3,0,0,0,0,0,0,0,2,1.3,0,0,0,0,0,0 -right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_pitch,0,0,0,1.5,1.5,1.5,1.5,1.5,1.5,1.5,1.5,2.3,2.3,2.3,1,1,1,1 -right_knee,0,0,-2.5,0,-1.2,-1.2,-1.2,-1.2,-1.2,-1.2,-1.3,-1.8,-2.1,-3,-3,-3,-1.7,-1.7 -right_ankle_pitch,0,0,1,0,0.5,0.5,0.5,0.5,0.5,0.5,1.5,1.5,1.5,1.5,1.5,2,0.9,0.9 -right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_left.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupback_left.csv deleted file mode 100644 index e0929e4e8..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_left.csv +++ /dev/null @@ -1,21 +0,0 @@ -time,0,2,3,3.3,3.7,4.5,5.5,6.5,7,7.5,8,8.5,9,10,10.5,11 -head_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5 -right_shoulder_pitch,0,0,1.6,1.6,1.6,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -right_shoulder_roll,0,0.5,0.5,0.5,0,0,0,0,0,0,0,0,0,0,0,0 -right_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_roll,0,0.4,0.4,0.4,0.4,0,0,0,0,0,0,0,0,0,0,0 -right_hip_pitch,0,-1,1,1,1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -right_knee,0,0,0,-1,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -right_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_shoulder_pitch,0,2.8,2.8,2.8,2.8,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -left_shoulder_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -left_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_pitch,0,1,-1,-1,-1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -left_knee,0,0,0,0,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -left_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_right.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupback_right.csv deleted file mode 100644 index eaa7717bb..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_right.csv +++ /dev/null @@ -1,21 +0,0 @@ -time,0,2,3,3.3,3.7,4.5,5.5,6.5,7,7.5,8,8.5,9,10,10.5,11 -head_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5 -left_shoulder_pitch,0,0,1.6,1.6,1.6,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -left_shoulder_roll,0,0.5,0.5,0.5,0,0,0,0,0,0,0,0,0,0,0,0 -left_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -left_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_roll,0,0.4,0.4,0.4,0.4,0,0,0,0,0,0,0,0,0,0,0 -left_hip_pitch,0,-1,1,1,1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -left_knee,0,0,0,-1,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -left_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_shoulder_pitch,0,2.8,2.8,2.8,2.8,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -right_shoulder_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_pitch,0,1,-1,-1,-1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -right_knee,0,0,0,0,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -right_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_roll.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupback_roll.csv deleted file mode 100644 index eaa7717bb..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_roll.csv +++ /dev/null @@ -1,21 +0,0 @@ -time,0,2,3,3.3,3.7,4.5,5.5,6.5,7,7.5,8,8.5,9,10,10.5,11 -head_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5 -left_shoulder_pitch,0,0,1.6,1.6,1.6,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -left_shoulder_roll,0,0.5,0.5,0.5,0,0,0,0,0,0,0,0,0,0,0,0 -left_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -left_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_roll,0,0.4,0.4,0.4,0.4,0,0,0,0,0,0,0,0,0,0,0 -left_hip_pitch,0,-1,1,1,1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -left_knee,0,0,0,-1,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -left_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_shoulder_pitch,0,2.8,2.8,2.8,2.8,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -right_shoulder_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_pitch,0,1,-1,-1,-1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -right_knee,0,0,0,0,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -right_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_roll_right.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupback_roll_right.csv deleted file mode 100644 index e0929e4e8..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/getupback_roll_right.csv +++ /dev/null @@ -1,21 +0,0 @@ -time,0,2,3,3.3,3.7,4.5,5.5,6.5,7,7.5,8,8.5,9,10,10.5,11 -head_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5,-0.5 -right_shoulder_pitch,0,0,1.6,1.6,1.6,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -right_shoulder_roll,0,0.5,0.5,0.5,0,0,0,0,0,0,0,0,0,0,0,0 -right_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_roll,0,0.4,0.4,0.4,0.4,0,0,0,0,0,0,0,0,0,0,0 -right_hip_pitch,0,-1,1,1,1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -right_knee,0,0,0,-1,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -right_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_shoulder_pitch,0,2.8,2.8,2.8,2.8,2.8,3.2,2,1.3,1.8,1,0.9,0.9,0.9,0.9,0.9 -left_shoulder_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_elbow,0,0,0,0,0,0,2,2,1.3,0,0,0,0,0,0,0 -left_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_pitch,0,1,-1,-1,-1,0,1,1.5,2.3,2.3,2.3,1.6,1.04,1.04,1.04,1.04 -left_knee,0,0,0,0,0,0,-1,-1.3,-1.8,-2.1,-3,-3,-3,-3,-2.3,-2.3 -left_ankle_pitch,0,0,0,0,0,0,1.5,1.5,1.5,1.5,1.5,1.8,2.2,2.2,1.4,1.4 -left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupfront.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupfront.csv index e8a19cf35..6b7573769 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2/getupfront.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2/getupfront.csv @@ -1,21 +1,21 @@ -time,0,0.5,1,1.5,2,2.5,3,3.5,4,4.5 +time,0,0.5,1,1.5,2,2.5,3,3.25,3.5,3.75 right_hip_yaw,0,0,0,0,0,0,0,0,0,0 left_hip_yaw,0,0,0,0,0,0,0,0,0,0 right_hip_roll,0,0,0,0,0,0,0,0,0,0 left_hip_roll,0,0,0,0,0,0,0,0,0,0 -right_hip_pitch,0,0,0,1,1.5,2,2,2,1,0.564 -left_hip_pitch,0,0,0,1,1.5,2,2,2,1,0.564 -right_knee,0,0,0,-1,-1.3,-3,-3,-3,-3, -1.176 -left_knee,0,0,0,-1,-1.3,-3,-3,-3,-3, -1.176 -right_ankle_pitch,0,0,1.1,3,3,3,3,3,1.2,0.613 -left_ankle_pitch,0,0,1.1,3,3,3,3,3,1.2,0.613 +right_hip_pitch,0,0,0,0.5,1.8,1.8,1,1,0.4,0.4 +left_hip_pitch,0,0,0,0.5,1.8,1.8,1,1,0.4,0.4 +right_knee,0,0,0,0,0,0,0,0,-0.8,-0.8 +left_knee,0,0,0,0,0,0,0,0,-0.8,-0.8 +right_ankle_pitch,1.7,1.7,1.7,1,0,-0.7,-0.7,-0.7,0.4,0.4 +left_ankle_pitch,1.7,1.7,1.7,1,0,-0.7,-0.7,-0.7,0.4,0.4 right_ankle_roll,0,0,0,0,0,0,0,0,0,0 left_ankle_roll,0,0,0,0,0,0,0,0,0,0 -right_shoulder_pitch,0,1.5,1.5,1.5,2,1.3,1.8,1.0,0.9,0 -left_shoulder_pitch,0,1.5,1.5,1.5,2,1.3,1.8,1.0,0.9,0 -right_elbow,0,3.14,3.14,2,2,1.3,0,0,0,0 -left_elbow,0,3.14,3.14,2,2,1.3,0,0,0,0 +right_shoulder_pitch,0,-1,0,1.57,1.57,1.57,1.57,1.57,0,0 +left_shoulder_pitch,0,-1,0,1.57,1.57,1.57,1.57,1.57,0,0 +right_elbow,0,1.7,1.7,1,0,0,0,0,0,0 +left_elbow,0,1.7,1.7,1,0,0,0,0,0,0 +right_shoulder_roll,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1 head_yaw,0,0,0,0,0,0,0,0,0,0 head_pitch,0,0,0,0,0,0,0,0,0,0 -right_shoulder_roll,0,0,0,0,0,0,0,0,0,0 -left_shoulder_roll,0,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/getupside.csv b/soccer_control/soccer_trajectories/trajectories/bez2/getupside.csv index 8f4122290..357761688 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2/getupside.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2/getupside.csv @@ -17,3 +17,5 @@ right_shoulder_roll,0,0 left_shoulder_roll,0,0 head_yaw,0,0 head_pitch,0,0 +right_shoulder_roll,0.05,0.05 +left_shoulder_roll,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/goalieleft.csv b/soccer_control/soccer_trajectories/trajectories/bez2/goalieleft.csv new file mode 100644 index 000000000..45ca4afe5 --- /dev/null +++ b/soccer_control/soccer_trajectories/trajectories/bez2/goalieleft.csv @@ -0,0 +1,21 @@ +time,0,0.5,5 +right_hip_yaw,0,0,0 +left_hip_yaw,0,0,0 +right_hip_roll,0,0,0 +left_hip_roll,0,0,0 +right_hip_pitch,0.564,0.564,0.564 +left_hip_pitch,0.564,0.564,0.564 +right_knee,-1.176,-1.176,-1.176 +left_knee,-1.176,-2.5,-1.176 +right_ankle_pitch,0.613,0.613,0.613 +left_ankle_pitch,0.613,1.2,0.613 +right_ankle_roll,0,0,0 +left_ankle_roll,0,0,0 +right_shoulder_pitch,3.14,3.14,3.14 +left_shoulder_pitch,3.14,3.14,3.14 +right_elbow,0,0,0 +left_elbow,0,0,0 +head_yaw,0,0,0 +head_pitch,0,0,0 +right_shoulder_roll,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/goalieright.csv b/soccer_control/soccer_trajectories/trajectories/bez2/goalieright.csv new file mode 100644 index 000000000..1cbdc7906 --- /dev/null +++ b/soccer_control/soccer_trajectories/trajectories/bez2/goalieright.csv @@ -0,0 +1,21 @@ +time,0,0.5,5 +right_hip_yaw,0,0,0 +left_hip_yaw,0,0,0 +right_hip_roll,0,0,0 +left_hip_roll,0,0,0 +right_hip_pitch,0.564,0.564,0.564 +left_hip_pitch,0.564,0.564,0.564 +right_knee,-1.176,-2.5,-1.176 +left_knee,-1.176,-1.176,-1.176 +right_ankle_pitch,0.613,1.2,0.613 +left_ankle_pitch,0.613,0.613,0.613 +right_ankle_roll,0,0,0 +left_ankle_roll,0,0,0 +right_shoulder_pitch,3.14,3.14,3.14 +left_shoulder_pitch,3.14,3.14,3.14 +right_elbow,0,0,0 +left_elbow,0,0,0 +head_yaw,0,0,0 +head_pitch,0,0,0 +right_shoulder_roll,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/rightkick.csv b/soccer_control/soccer_trajectories/trajectories/bez2/rightkick.csv index a9d12e1e9..6f3d9e005 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2/rightkick.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2/rightkick.csv @@ -1,20 +1,22 @@ -time,0.5,1.5,2,2.5,5,8,8.1,8.3 -left_hip_yaw,0,0,0,0,0,0,0,0 -left_hip_roll,0.1,0.1,-0.15,-0.15,-0.15,-0.15,-0.15,-0.15 -left_hip_pitch,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25 -left_knee,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4 -left_ankle_pitch,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2 -left_ankle_roll,-0.1,-0.1,0.15,0.15,0.25,0.25,0.25,0.25 -left_shoulder_pitch,0,0,0,0,-1,-1,1,1 -left_shoulder_roll,2,2,2,2,2,2,2,2 -right_hip_yaw,0,0,0,0,0,0,0,0 -right_hip_roll,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4 -right_hip_pitch,0.25,0.25,0.25,0.25,0.3,0.3,1.5,1 -right_knee,-0.4,-0.4,-0.4,-0.4,-1,-1,-1,-1 -right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,0.5,0,0 -right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2 -right_shoulder_pitch,0,0,0,0,1,1,-1,-1 -right_shoulder_roll,2,2,2,2,2,2,2,2 -comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance -head_yaw,0,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,0,0 +time,0.69,0.7,0.75,1,1.3,1.4,1.45,1.7,2 +left_hip_yaw,0,0,0,0,0,0,0,0,0 +left_hip_roll,0.1,0.1,-0.15,-0.15,-0.25,0.1,0.1,0.3,0.1 +left_hip_pitch,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25 +left_knee,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4 +left_ankle_pitch,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2 +left_ankle_roll,-0.2,-0.2,0.15,0.15,0.25,0.25,0.25,-0.1,-0.1 +left_shoulder_pitch,0,0,0,0,-1,-1,1,1,0 +left_shoulder_roll,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1 +left_elbow,2,2,2,2,2,2,2,2,2 +right_hip_yaw,0,0,0,0,0,0,0,0,0 +right_hip_roll,0.2,0.2,0.2,0.2,0.4,0.4,0.4,0.1,0.1 +right_hip_pitch,0.25,0.25,0.25,0.25,0.3,0.6,1.5,0.25,0.25 +right_knee,-0.4,-0.4,-0.4,-0.4,-1,-2,0,-0.4,-0.4 +right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,1.2,0.5,0.2,0.2 +right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.1,-0.1 +right_shoulder_pitch,0,0,0,0,1,1,-1,-1,0 +right_elbow,2,2,2,2,2,2,2,2,2 +comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance,stance +right_shoulder_roll,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05 +head_yaw,0,0,0,0,0,0,0,0,0 +head_pitch,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/rightkick_2.csv b/soccer_control/soccer_trajectories/trajectories/bez2/rightkick_2.csv deleted file mode 100644 index 873c4645b..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/rightkick_2.csv +++ /dev/null @@ -1,20 +0,0 @@ -time,1,1.2,1.45,1.5,1.7,1.9,2.1,2.3,2.5 -left_hip_yaw,0,0,0,0,0,0,0,0,0 -left_hip_roll,0.1,0.1,0.15,0.15,0.15,0.15,0.15,0.15,0.1 -left_hip_pitch,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25 -left_knee,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4 -left_ankle_pitch,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2 -left_ankle_roll,-0.1,-0.1,0.15,0.15,0.25,0.25,0.4,0.4,-0.1 -left_shoulder_pitch,0,0,0,0,-1,-1,1,1,0 -left_shoulder_roll,2,2,2,2,2,2,2,2,2 -right_hip_yaw,0,0,0,0,0,0,0,0,0 -right_hip_roll,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4,0.1 -right_hip_pitch,0.25,0.25,0.25,0.25,0.3,0.3,1,0.5,0.25 -right_knee,-0.4,-0.4,-0.4,-0.4,-0.6,-0.8,-0.8,-0.8,-0.4 -right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,0.5,0,0,0.2 -right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2,-0.1 -right_shoulder_pitch,0,0,0,0,1,1,-1,-1,0 -right_shoulder_roll,2,2,2,2,2,2,2,2,2 -comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance,stance -head_yaw,0,0,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/rightkick_old.csv b/soccer_control/soccer_trajectories/trajectories/bez2/rightkick_old.csv deleted file mode 100644 index cf9dc04dc..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/rightkick_old.csv +++ /dev/null @@ -1,18 +0,0 @@ -time,2,5,9,7,7.3,8 -left_hip_yaw,0,0,0,0,0,0 -left_hip_roll,-0.05,-0.3,-0.5,-0.2,-0.2,-0.2 -left_hip_pitch,0.25,0.25,0.25,0.25,0.25,0.25 -left_knee,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4 -left_ankle_pitch,0.2,0.2,0.2,0.2,0.2,0.2 -left_ankle_roll,0.05,0.2,0.4,0.2,0.2,0.2 -right_hip_yaw,0,0,0,0,0,0 -right_hip_roll,-0.05,-0.3,-1.5,-0.3,-0.3,-0.3 -right_hip_pitch,0.25,0.4,0.7,-0.3,1.5,0.6 -right_knee,-0.4,-0.5,-1.4,-2,-0.5,-1.4 -right_ankle_pitch,0.2,0.3,0.7,0.9,0.5,0.8 -right_ankle_roll,0.05,0.3,0.3,0.2,0.2,0.2 -right_shoulder_pitch,0,0,0,0,0,0 -right_shoulder_roll,2,2,2,2,2,2 -left_shoulder_pitch,0,0,0,0,0,0 -left_shoulder_roll,2,2,2,2,2,2 -comment,initial pose,lean,Lean more and lift leg,wind back,kick,stand position diff --git a/soccer_control/soccer_trajectories/trajectories/bez2/sidekick.csv b/soccer_control/soccer_trajectories/trajectories/bez2/sidekick.csv deleted file mode 100644 index ac304f487..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2/sidekick.csv +++ /dev/null @@ -1,20 +0,0 @@ -time,0.5,5.5,5.7,10 -left_hip_yaw,0,0,0,0 -left_hip_roll,0,0,0,0 -left_hip_pitch,0,0,0,0 -left_knee,0,0,0,0 -left_ankle_pitch,0,0,0,0 -left_ankle_roll,0,0,0,0 -left_shoulder_pitch,0,0,0,0 -left_shoulder_roll,0,0,0,0 -right_hip_yaw,0,0,0,0 -right_hip_roll,0,0,0,0 -right_hip_pitch,0,0,0,0 -right_knee,0,0,0,0 -right_ankle_pitch,0,0,0,0 -right_ankle_roll,0,0,0,0 -right_shoulder_pitch,0,0,0,0 -right_shoulder_roll,0,0,0,0 -comment,0,0,0,0 -head_yaw,0,0,0,0 -head_pitch,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv index f628d70ac..75e3fc25c 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback.csv @@ -1,21 +1,21 @@ -time,0,0.75,1.2,1.4,1.6,2,2.75,3.5,4.25,5,5.75,6.5,7.25,8,8.75 -right_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_hip_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_hip_roll,0,0,0,0,0,0.2,0.2,0,0,0,0,0,0,0,0 -left_hip_roll,0,0,0,0,0,0.2,0.2,0,0,0,0,0,0,0,0 -right_hip_pitch,0,0,0,0,0,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564 -left_leg_motor_2,0,0,0,0,0,0,0,0,1,1.5,1.8,1.8,1.8,1.7,0.564 -right_knee,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176 -left_knee,0,0,-2.5,-2.5,0,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176 -right_ankle_pitch,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613 -left_ankle_pitch,0,0,-0.77,-0.77,-0.77,-0.77,0,1.1,1.3,1.3,1.3,1.3,1.2,1.2,0.613 -right_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_ankle_roll,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_shoulder_pitch,0,0,0,0,-1.57,-1.57,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0 -left_shoulder_pitch,0,-1.57,-1.57,-1.57,-1.57,-1.57,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0 -right_shoulder_roll,0,0,0,0,0,0,3.14,3.14,2,2,1.3,0,0,0,0 -left_shoulder_roll,0,1.57,1.57,0,0,0,3.14,3.14,2,2,1.3,0,0,0,0 -head_yaw,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -head_pitch,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -right_elbow,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 -left_elbow,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 +time,0,0.5,1,1.5,2.25,2.5,2,2.75,3 +right_hip_yaw,0,0,0,0,0,0,0,0,0 +left_hip_yaw,0,0,0,0,0,0,0,0,0 +right_hip_roll,0,0,0,0,0,0,0,0,0 +left_hip_roll,0,0,0,0,0,0,0,0,0 +right_hip_pitch,0,1.3,0.2,-0.4,-0.4,-0.8,-0.4,0.4,0.4 +left_hip_pitch,0,1.3,0.2,-0.4,-0.4,-0.8,-0.4,0.4,0.4 +right_knee,0,-2,-2,-1.8,-1.8,-1.8,-1.8,-1.8,-0.8 +left_knee,0,-2,-2,-1.8,-1.8,-1.8,-1.8,-1.8,-0.8 +right_ankle_pitch,0,0.6,1.5,1.2,1,1.5,1,1,0.4 +left_ankle_pitch,0,0.6,1.5,1.2,1,1.5,1,1,0.4 +right_ankle_roll,0,0,0,0,0,0,0,0,0 +left_ankle_roll,0,0,0,0,0,0,0,0,0 +right_shoulder_pitch,0,-0.5,-1.6,-2.2,-1,-1,-2,-1,0 +left_shoulder_pitch,0,-0.5,-1.6,-2.2,-1,-1,-2,-1,0 +right_elbow,0,0.7,1.5,1.8,0,0,1.2,0,0 +left_elbow,0,0.7,1.5,1.8,0,0,1.2,0,0 +head_yaw,0,0,0,0,0,0,0,0,0 +head_pitch,0,0,0,0,0,0,0,0,0 +right_shoulder_roll,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv deleted file mode 100644 index 2ef76a947..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupback_real.csv +++ /dev/null @@ -1,19 +0,0 @@ -time,0,0.75,1.2,1.4,1.6,2,2.75,3.5,4.25,5,5.75,6.5,7.25,8,8.75,9.25,10.25,11,11.75 -right_hip_yaw,0.0,1.57,1.57,1.57,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 -left_hip_yaw,0.0,-1.57,-1.57,-1.57,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 -right_hip_roll,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.2,0.2,0.0,0.0,0.0,0,0,0,0 -left_hip_roll,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.2,0.2,0.0,0.0,0.0,0,0,0,0 -right_hip_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.5,1.8,1.8,1.8,1.7 -left_leg_motor_2,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.5,1.8,1.8,1.8,1.7 -right_knee,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6 -left_knee,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,-1.3,-1.8,-2.2,-2.6,-2.6 -right_ankle_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2 -left_ankle_pitch,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,-0.77,-0.77,0.0,1.1,1.3,1.3,1.3,1.3,1.2,1.2 -right_ankle_roll,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0 -left_ankle_roll,1.57,1.57,1.57,1.57,1.57,-1.57,-1.57,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0,0,0,0 -right_shoulder_pitch,3.9,3.9,3.9,1.57,1.57,1.57,1.57,3.9,3.9,3.9,3.9,0.0,1.5,1.5,2.0,1.3,1.8,1.0,0.9 -left_shoulder_pitch,3.9,3.9,3.9,1.57,1.57,1.57,1.57,3.9,3.9,3.9,3.9,0.0,1.5,1.5,2.0,1.3,1.8,1.0,0.9 -right_elbow,0,0,0,0,0,0,0.0,0.0,0.0,0.0,0.0,3.14,3.14,2.0,2.0,1.3,0,0,0 -left_elbow,0,0,0,0,0,0,0.0,0.0,0.0,0.0,0.0,3.14,3.14,2.0,2.0,1.3,0,0,0 -head_yaw,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 -head_pitch,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 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv index 74552c003..6fcb576f7 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupfront.csv @@ -1,21 +1,21 @@ -time,0,0.75,1.5,2.25,3,3.75,4.5,5.25,6,6.75 +time,0,0.5,1,1.5,2,2.5,3,3.25,3.5,3.75 right_hip_yaw,0,0,0,0,0,0,0,0,0,0 left_hip_yaw,0,0,0,0,0,0,0,0,0,0 right_hip_roll,0,0,0,0,0,0,0,0,0,0 left_hip_roll,0,0,0,0,0,0,0,0,0,0 -right_hip_pitch,0,0,0,1,1.5,2,2,2,1.7,0.564 -left_leg_motor_2,0,0,0,1,1.5,2,2,2,1.7,0.564 -right_knee,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176 -left_knee,0,0,0,-1,-1.3,-1.8,-2.2,-2.6,-2.6, -1.176 -right_ankle_pitch,0,0,1.1,1.5,1.5,1.5,1.5,1.2,1.2,0.613 -left_ankle_pitch,0,0,1.1,1.5,1.5,1.5,1.5,1.2,1.2,0.613 +right_hip_pitch,0,0,0,0.5,1.8,1.8,1,1,1,0.4 +left_hip_pitch,0,0,0,0.5,1.8,1.8,1,1,1,0.4 +right_knee,0,0,0,0,0,0,0,0,0,-0.8 +left_knee,0,0,0,0,0,0,0,0,0,-0.8 +right_ankle_pitch,1.7,1.7,1.7,1,0,-0.7,-0.7,-0.7,-0.7,0.4 +left_ankle_pitch,1.7,1.7,1.7,1,0,-0.7,-0.7,-0.7,-0.7,0.4 right_ankle_roll,0,0,0,0,0,0,0,0,0,0 left_ankle_roll,0,0,0,0,0,0,0,0,0,0 -right_shoulder_pitch,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0 -left_shoulder_pitch,0,0,1.5,1.5,2,1.3,1.8,1.0,0.9,0 -right_shoulder_roll,0,3.14,3.14,2,2,1.3,0,0,0,0 -left_shoulder_roll,0,3.14,3.14,2,2,1.3,0,0,0,0 +right_shoulder_pitch,0,-1,0,1.57,1.57,1.57,1.57,1.57,1.57,0 +left_shoulder_pitch,0,-1,0,1.57,1.57,1.57,1.57,1.57,1.57,0 +right_elbow,0,1.7,1.7,1,0,0,0,0,0,0 +left_elbow,0,1.7,1.7,1,0,0,0,0,0,0 +right_shoulder_roll,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1 head_yaw,0,0,0,0,0,0,0,0,0,0 head_pitch,0,0,0,0,0,0,0,0,0,0 -right_elbow,0,0,0,0,0,0,0,0,0,0 -left_elbow,0,0,0,0,0,0,0,0,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupside.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupside.csv deleted file mode 100644 index ceb190ca7..000000000 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupside.csv +++ /dev/null @@ -1,19 +0,0 @@ -time,0,0.5 -right_hip_yaw,0,0 -left_hip_yaw,0,0 -right_hip_roll,0,0 -left_hip_roll,0,0 -right_hip_pitch,0,0 -left_leg_motor_2,0,0 -right_knee,0,0 -left_knee,0,0 -right_ankle_pitch,0,0 -left_ankle_pitch,0,0 -right_ankle_roll,0,0 -left_ankle_roll,0,0 -right_shoulder_pitch,0,-1.5708 -left_shoulder_pitch,0,-1.5708 -right_elbow,0,0 -left_elbow,0,0 -head_yaw,0,0 -head_pitch,0,0 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupsideleft.csv similarity index 62% rename from soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv rename to soccer_control/soccer_trajectories/trajectories/bez2_sim/getupsideleft.csv index 0486a908c..cf1fd8567 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/fix_angle_test.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupsideleft.csv @@ -1,21 +1,21 @@ -time,0,1.5,3 +time,0,0.5,0.75 right_hip_yaw,0,0,0 left_hip_yaw,0,0,0 right_hip_roll,0,0,0 left_hip_roll,0,0,0 right_hip_pitch,0,0,0 -left_leg_motor_2,0,0,0 +left_hip_pitch,0,0,0 right_knee,0,0,0 left_knee,0,0,0 right_ankle_pitch,0,0,0 left_ankle_pitch,0,0,0 right_ankle_roll,0,0,0 left_ankle_roll,0,0,0 -right_shoulder_pitch,0,3.14,3.14 -left_shoulder_pitch,0,3.14,3.14 -right_elbow,0,0,0 -left_elbow,0,0,0 -head_yaw,0,1,-1 -head_pitch,0,0,0 +right_shoulder_pitch,0,0,0 +left_shoulder_pitch,0,-0.785,0 right_shoulder_roll,0,0,0 left_shoulder_roll,0,0,0 +head_yaw,0,0,0 +head_pitch,0,0,0 +right_shoulder_roll,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupsideright.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupsideright.csv new file mode 100644 index 000000000..91060a53d --- /dev/null +++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/getupsideright.csv @@ -0,0 +1,21 @@ +time,0,0.5,0.75 +right_hip_yaw,0,0,0 +left_hip_yaw,0,0,0 +right_hip_roll,0,0,0 +left_hip_roll,0,0,0 +right_hip_pitch,0,0,0 +left_hip_pitch,0,0,0 +right_knee,0,0,0 +left_knee,0,0,0 +right_ankle_pitch,0,0,0 +left_ankle_pitch,0,0,0 +right_ankle_roll,0,0,0 +left_ankle_roll,0,0,0 +right_shoulder_pitch,0,-0.785,0 +left_shoulder_pitch,0,0,0 +right_shoulder_roll,0,0,0 +left_shoulder_roll,0,0,0 +head_yaw,0,0,0 +head_pitch,0,0,0 +right_shoulder_roll,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv index 6c544846e..45ca4afe5 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieleft.csv @@ -4,7 +4,7 @@ left_hip_yaw,0,0,0 right_hip_roll,0,0,0 left_hip_roll,0,0,0 right_hip_pitch,0.564,0.564,0.564 -left_leg_motor_2,0.564,0.564,0.564 +left_hip_pitch,0.564,0.564,0.564 right_knee,-1.176,-1.176,-1.176 left_knee,-1.176,-2.5,-1.176 right_ankle_pitch,0.613,0.613,0.613 @@ -17,3 +17,5 @@ right_elbow,0,0,0 left_elbow,0,0,0 head_yaw,0,0,0 head_pitch,0,0,0 +right_shoulder_roll,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv index 8fba6b668..1cbdc7906 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/goalieright.csv @@ -4,7 +4,7 @@ left_hip_yaw,0,0,0 right_hip_roll,0,0,0 left_hip_roll,0,0,0 right_hip_pitch,0.564,0.564,0.564 -left_leg_motor_2,0.564,0.564,0.564 +left_hip_pitch,0.564,0.564,0.564 right_knee,-1.176,-2.5,-1.176 left_knee,-1.176,-1.176,-1.176 right_ankle_pitch,0.613,1.2,0.613 @@ -17,3 +17,5 @@ right_elbow,0,0,0 left_elbow,0,0,0 head_yaw,0,0,0 head_pitch,0,0,0 +right_shoulder_roll,0.05,0.05,0.05 +left_shoulder_roll,0.1,0.1,0.1 diff --git a/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv b/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv index 6a75c4e68..6f3d9e005 100644 --- a/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv +++ b/soccer_control/soccer_trajectories/trajectories/bez2_sim/rightkick.csv @@ -1,20 +1,22 @@ -time,0.69,0.7,0.75,1,1.2,1.4,1.41,1.7,2 +time,0.69,0.7,0.75,1,1.3,1.4,1.45,1.7,2 left_hip_yaw,0,0,0,0,0,0,0,0,0 -left_hip_roll,0.1,0.1,-0.15,-0.15,-0.25,-0.25,-0.25,-0.25,0.1 -left_leg_motor_2,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25 +left_hip_roll,0.1,0.1,-0.15,-0.15,-0.25,0.1,0.1,0.3,0.1 +left_hip_pitch,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25,0.25 left_knee,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4,-0.4 left_ankle_pitch,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2 -left_ankle_roll,-0.1,-0.1,0.15,0.15,0.25,0.25,0.25,0.25,-0.1 +left_ankle_roll,-0.2,-0.2,0.15,0.15,0.25,0.25,0.25,-0.1,-0.1 left_shoulder_pitch,0,0,0,0,-1,-1,1,1,0 +left_shoulder_roll,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1,0.1 left_elbow,2,2,2,2,2,2,2,2,2 right_hip_yaw,0,0,0,0,0,0,0,0,0 -right_hip_roll,0.1,0.1,0.2,0.2,0.4,0.4,0.4,0.4,0.1 -right_hip_pitch,0.25,0.25,0.25,0.25,0.3,0.3,1.5,1,0.25 -right_knee,-0.4,-0.4,-0.4,-0.4,-1,-1,-1,-1,-0.4 -right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,0.5,0,0,0.2 -right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.2,-0.1 +right_hip_roll,0.2,0.2,0.2,0.2,0.4,0.4,0.4,0.1,0.1 +right_hip_pitch,0.25,0.25,0.25,0.25,0.3,0.6,1.5,0.25,0.25 +right_knee,-0.4,-0.4,-0.4,-0.4,-1,-2,0,-0.4,-0.4 +right_ankle_pitch,0.2,0.2,0.2,0.2,0.5,1.2,0.5,0.2,0.2 +right_ankle_roll,-0.1,-0.1,-0.1,-0.1,-0.2,-0.2,-0.2,-0.1,-0.1 right_shoulder_pitch,0,0,0,0,1,1,-1,-1,0 right_elbow,2,2,2,2,2,2,2,2,2 comment,stance,x,lean to the side,x,lift leg up,x,kick,step to balance,stance +right_shoulder_roll,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05,0.05 head_yaw,0,0,0,0,0,0,0,0,0 head_pitch,0,0,0,0,0,0,0,0,0 diff --git a/soccer_description/assembly_description/CMakeLists.txt b/soccer_description/assembly_description/CMakeLists.txt index 4910b4ac8..d266b8dab 100644 --- a/soccer_description/assembly_description/CMakeLists.txt +++ b/soccer_description/assembly_description/CMakeLists.txt @@ -4,10 +4,10 @@ project(assembly_description) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS +## Find ros2 macros and libraries +## if COMPONENTS list like find_package(ros2 REQUIRED COMPONENTS xyz) +## is used, also find other ros2 packages +find_package(ros2 REQUIRED COMPONENTS rospy ) @@ -17,8 +17,8 @@ find_package(catkin REQUIRED COMPONENTS ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +## See http://ros.org/doc/api/ros2/html/user_guide/setup_dot_py.html +# ros2_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -36,9 +36,9 @@ find_package(catkin REQUIRED COMPONENTS ## * add a exec_depend tag for "message_runtime" ## * In this file (CMakeLists.txt): ## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) +## find_package(ros2 REQUIRED COMPONENTS ...) ## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) +## ros2_package(CATKIN_DEPENDS ...) ## * uncomment the add_*_files sections below as needed ## and list every .msg/.srv/.action file to be processed ## * uncomment the generate_messages entry below @@ -81,7 +81,7 @@ find_package(catkin REQUIRED COMPONENTS ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" ## * In this file (CMakeLists.txt): ## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) +## find_package(ros2 REQUIRED COMPONENTS ...) ## * uncomment the "generate_dynamic_reconfigure_options" section below ## and list every .cfg file to be processed @@ -92,15 +92,15 @@ find_package(catkin REQUIRED COMPONENTS # ) ################################### -## catkin specific configuration ## +## ros2 specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package +## The ros2_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDE_DIRS: uncomment this if your package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need +## CATKIN_DEPENDS: ros2_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( +ros2_package( # INCLUDE_DIRS include # LIBRARIES fusion2urdf # CATKIN_DEPENDS rospy @@ -115,7 +115,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( # include - ${catkin_INCLUDE_DIRS} + ${ros2_INCLUDE_DIRS} ) ## Declare a C++ library @@ -126,10 +126,10 @@ include_directories( ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ros2_EXPORTED_TARGETS}) ## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context +## With ros2_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/fusion2urdf_node.cpp) @@ -141,19 +141,19 @@ include_directories( ## Add cmake target dependencies of the executable ## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ros2_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against # target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} +# ${ros2_LIBRARIES} # ) ############# ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html +# all install targets should use ros2 DESTINATION variables +# See http://ros.org/doc/api/ros2/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination @@ -188,10 +188,10 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_fusion2urdf.cpp) +# ros2_add_gtest(${PROJECT_NAME}-test test/test_fusion2urdf.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +# ros2_add_nosetests(test) diff --git a/soccer_description/assembly_description/package.xml b/soccer_description/assembly_description/package.xml index a63b2a722..598c67032 100644 --- a/soccer_description/assembly_description/package.xml +++ b/soccer_description/assembly_description/package.xml @@ -29,7 +29,7 @@ - + @@ -41,14 +41,14 @@ - + - catkin + ros2 rospy rospy rospy diff --git a/soccer_description/assembly_description/urdf/assembly.urdf b/soccer_description/assembly_description/urdf/assembly.urdf index 46e0f5075..752eebbb1 100644 --- a/soccer_description/assembly_description/urdf/assembly.urdf +++ b/soccer_description/assembly_description/urdf/assembly.urdf @@ -970,5 +970,20 @@ 0.2 0.2 + + + + + + + + + + + + + + + diff --git a/soccer_description/assembly_description/urdf/assembly.xacro b/soccer_description/assembly_description/urdf/assembly.xacro index afeb3f343..edc6e44b7 100644 --- a/soccer_description/assembly_description/urdf/assembly.xacro +++ b/soccer_description/assembly_description/urdf/assembly.xacro @@ -6,583 +6,634 @@ - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/soccer_description/assembly_description/urdf/robot.urdf b/soccer_description/assembly_description/urdf/robot.urdf index 5be771b55..977ee3bbf 100644 --- a/soccer_description/assembly_description/urdf/robot.urdf +++ b/soccer_description/assembly_description/urdf/robot.urdf @@ -616,6 +616,20 @@ - + + + + + + + + + + + + + + + diff --git a/soccer_description/assembly_description/urdf/robot1.urdf b/soccer_description/assembly_description/urdf/robot1.urdf new file mode 100644 index 000000000..bfe197a6b --- /dev/null +++ b/soccer_description/assembly_description/urdf/robot1.urdf @@ -0,0 +1,635 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/soccer_description/bez1_description/CMakeLists.txt b/soccer_description/bez1_description/CMakeLists.txt index f6b2631af..69ec6fee0 100644 --- a/soccer_description/bez1_description/CMakeLists.txt +++ b/soccer_description/bez1_description/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.0.2) project(bez1_description) find_package( - catkin + ros2 REQUIRED urdf xacro @@ -12,7 +12,7 @@ find_package( rospy ) -catkin_package( +ros2_package( CATKIN_DEPENDS urdf xacro diff --git a/soccer_description/bez1_description/package.xml b/soccer_description/bez1_description/package.xml index e7cf05698..7a1f0b79e 100644 --- a/soccer_description/bez1_description/package.xml +++ b/soccer_description/bez1_description/package.xml @@ -7,7 +7,7 @@ BSD Jason Wang - catkin + ros2 urdf xacro dynamic_reconfigure diff --git a/soccer_description/bez1_description/scripts/convert_stl2dae.py b/soccer_description/bez1_description/scripts/convert_stl2dae.py index da9199804..b144a4169 100644 --- a/soccer_description/bez1_description/scripts/convert_stl2dae.py +++ b/soccer_description/bez1_description/scripts/convert_stl2dae.py @@ -9,9 +9,9 @@ os.chdir(args.folder) for stl_fileName in glob.glob("*.stl"): - conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "obj" + conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "dae" os.system(conversion_command) for stl_fileName in glob.glob("*.STL"): - conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "obj" + conversion_command = "meshlabserver -i " + stl_fileName + " -o " + stl_fileName[:-3] + "dae" os.system(conversion_command) diff --git a/soccer_description/bez1_description/scripts/modify_proto.py b/soccer_description/bez1_description/scripts/modify_proto.py index 9a543dc67..f56c62db0 100644 --- a/soccer_description/bez1_description/scripts/modify_proto.py +++ b/soccer_description/bez1_description/scripts/modify_proto.py @@ -4,10 +4,10 @@ print("Modifying .proto file") home = expanduser("~") -input_file = home + "/catkin_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" +input_file = home + "/ros2_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" target_file = ( - home + "/catkin_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" -) # home + '/catkin_ws/src/soccerbot/soccer_webots/robocup/protos/Bez2.proto' + home + "/ros2_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" +) # home + '/ros2_ws/src/soccerbot/soccer_webots/robocup/protos/Bez2.proto' text = "" with open(input_file, "r") as proto_file: text = proto_file.read() @@ -30,7 +30,7 @@ metalness 0 } geometry DEF torso Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/torso.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/torso.stl" } }""" @@ -84,7 +84,7 @@ metalness 0 } geometry DEF torso Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/torso.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/torso.stl" } } """ @@ -228,7 +228,7 @@ Shape { appearance USE steel_satin geometry DEF camera_point_1 Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez2_description/meshes/camera_point_1.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez2_description/meshes/camera_point_1.stl" } } ] @@ -304,7 +304,7 @@ Shape { appearance USE steel_satin geometry DEF imu_1 Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez2_description/meshes/imu_1.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez2_description/meshes/imu_1.stl" } } ] @@ -413,7 +413,7 @@ text = text.replace(target_text, replace_text) # update STL paths - text = text.replace(home + "/catkin_ws/src/soccerbot/bez2_description/meshes/", "") + text = text.replace(home + "/ros2_ws/src/soccerbot/bez2_description/meshes/", "") # set hands target_text = """children [ diff --git a/soccer_description/bez1_description/scripts/search.py b/soccer_description/bez1_description/scripts/search.py index 483a9d520..e97e5ef94 100644 --- a/soccer_description/bez1_description/scripts/search.py +++ b/soccer_description/bez1_description/scripts/search.py @@ -3,8 +3,8 @@ home = expanduser("~") -input_file = home + "/catkin_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" -target_file = home + "/catkin_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" +input_file = home + "/ros2_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" +target_file = home + "/ros2_ws/src/soccerbot/bez1_description/urdf/Bez1.proto" text = "" key = "device" diff --git a/soccer_description/bez1_description/scripts/simplify_stl.py b/soccer_description/bez1_description/scripts/simplify_stl.py index 5214ba011..30729c94a 100644 --- a/soccer_description/bez1_description/scripts/simplify_stl.py +++ b/soccer_description/bez1_description/scripts/simplify_stl.py @@ -11,8 +11,8 @@ # iterate over files in # that directory -directory = home + "/catkin_ws/src/soccerbot/bez1_description/meshes" -new_dir = home + "/catkin_ws/src/soccerbot/bez1_description/Bez1_meshes" +directory = home + "/ros2_ws/src/soccerbot/bez1_description/meshes" +new_dir = home + "/ros2_ws/src/soccerbot/bez1_description/Bez1_meshes" if not os.path.exists(new_dir): os.mkdir(new_dir) print(f"----------------------- creating {new_dir}") diff --git a/soccer_description/bez1_description/scripts/split_file.py b/soccer_description/bez1_description/scripts/split_file.py index 987d340ea..9c3734474 100644 --- a/soccer_description/bez1_description/scripts/split_file.py +++ b/soccer_description/bez1_description/scripts/split_file.py @@ -5,9 +5,9 @@ target_file = ( home - + "/catkin_ws/src/soccerbot/bez1_description/urdf/Bez1Robocup.proto" - # home + "/catkin_ws/src/soccerbot/soccer_webots/robocup/protos/Bez1.proto" -) # home + '/catkin_ws/src/soccerbot/soccer_webots/robocup/protos/Bez2.proto' + + "/ros2_ws/src/soccerbot/bez1_description/urdf/Bez1Robocup.proto" + # home + "/ros2_ws/src/soccerbot/soccer_webots/robocup/protos/Bez1.proto" +) # home + '/ros2_ws/src/soccerbot/soccer_webots/robocup/protos/Bez2.proto' with open(target_file, "w") as proto_file: diff --git a/soccer_description/bez1_description/urdf/Bez1.proto b/soccer_description/bez1_description/urdf/Bez1.proto index be9c9f2a4..f93c20b45 100644 --- a/soccer_description/bez1_description/urdf/Bez1.proto +++ b/soccer_description/bez1_description/urdf/Bez1.proto @@ -2,7 +2,7 @@ # license: Apache License 2.0 # license url: http://www.apache.org/licenses/LICENSE-2.0 # This is a proto file for Webots for the Bez1 -# Extracted from: /home/manx52/catkin_ws/src/soccerbot/bez1_description/urdf/bez1_simple.urdf +# Extracted from: /home/manx52/ros2_ws/src/soccerbot/bez1_description/urdf/bez1_simple.urdf PROTO Bez1 [ field SFVec3f translation 0 0 0 @@ -33,7 +33,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF torso Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/torso.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/torso.stl" } } GPS { @@ -74,7 +74,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF left_bicep Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_bicep.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_bicep.stl" } } HingeJoint { @@ -105,7 +105,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF left_forearm Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_forearm.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_forearm.stl" } } ] @@ -175,7 +175,7 @@ PROTO Bez1 [ Shape { appearance USE white geometry DEF right_bicep Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_bicep.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_bicep.stl" } } HingeJoint { @@ -202,7 +202,7 @@ PROTO Bez1 [ Shape { appearance USE purple geometry DEF right_forearm Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_forearm.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_forearm.stl" } } ] @@ -276,7 +276,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF left_hip_side Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_hip_side.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_hip_side.stl" } } HingeJoint { @@ -305,7 +305,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF left_hip_front Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_hip_front.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_hip_front.stl" } } HingeJoint { @@ -335,7 +335,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF left_thigh Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_thigh.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_thigh.stl" } } HingeJoint { @@ -369,7 +369,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF left_calve Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_calve.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_calve.stl" } } ] @@ -399,7 +399,7 @@ PROTO Bez1 [ Shape { appearance USE purple geometry DEF left_ankle Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_ankle.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_ankle.stl" } } HingeJoint { @@ -428,7 +428,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF left_foot Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_foot.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/left_foot.stl" } } ] @@ -582,7 +582,7 @@ PROTO Bez1 [ Shape { appearance USE cyan geometry DEF right_hip_side Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_hip_side.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_hip_side.stl" } } HingeJoint { @@ -608,7 +608,7 @@ PROTO Bez1 [ Shape { appearance USE blue geometry DEF right_hip_front Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_hip_front.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_hip_front.stl" } } HingeJoint { @@ -634,7 +634,7 @@ PROTO Bez1 [ Shape { appearance USE red geometry DEF right_thigh Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_thigh.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_thigh.stl" } } HingeJoint { @@ -664,7 +664,7 @@ PROTO Bez1 [ Shape { appearance USE yellow geometry DEF right_calve Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_calve.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_calve.stl" } } ] @@ -694,7 +694,7 @@ PROTO Bez1 [ Shape { appearance USE purple geometry DEF right_ankle Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_ankle.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_ankle.stl" } } HingeJoint { @@ -720,7 +720,7 @@ PROTO Bez1 [ Shape { appearance USE green geometry DEF right_foot Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_foot.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/right_foot.stl" } } ] @@ -878,7 +878,7 @@ PROTO Bez1 [ metalness 0 } geometry DEF neck Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/neck.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/neck.stl" } } HingeJoint { @@ -906,7 +906,7 @@ PROTO Bez1 [ Shape { appearance USE silver geometry DEF head Mesh { - url "/home/manx52/catkin_ws/src/soccerbot/bez1_description/bez1_description/meshes/head.stl" + url "/home/manx52/ros2_ws/src/soccerbot/bez1_description/bez1_description/meshes/head.stl" } } Solid { diff --git a/soccer_description/bez1_description/urdf/bez1.urdf b/soccer_description/bez1_description/urdf/bez1.urdf index 2d70e1864..2f41f152b 100644 --- a/soccer_description/bez1_description/urdf/bez1.urdf +++ b/soccer_description/bez1_description/urdf/bez1.urdf @@ -887,6 +887,21 @@ 0.2 0.2 + + + + + + + + + + + + + + + diff --git a/soccer_description/bez2_description/CMakeLists.txt b/soccer_description/bez2_description/CMakeLists.txt index 56a48944f..10c022fad 100644 --- a/soccer_description/bez2_description/CMakeLists.txt +++ b/soccer_description/bez2_description/CMakeLists.txt @@ -4,10 +4,10 @@ project(bez2_description) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS +## Find ros2 macros and libraries +## if COMPONENTS list like find_package(ros2 REQUIRED COMPONENTS xyz) +## is used, also find other ros2 packages +find_package(ros2 REQUIRED COMPONENTS rospy ) @@ -17,8 +17,8 @@ find_package(catkin REQUIRED COMPONENTS ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +## See http://ros.org/doc/api/ros2/html/user_guide/setup_dot_py.html +# ros2_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -36,9 +36,9 @@ find_package(catkin REQUIRED COMPONENTS ## * add a exec_depend tag for "message_runtime" ## * In this file (CMakeLists.txt): ## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) +## find_package(ros2 REQUIRED COMPONENTS ...) ## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) +## ros2_package(CATKIN_DEPENDS ...) ## * uncomment the add_*_files sections below as needed ## and list every .msg/.srv/.action file to be processed ## * uncomment the generate_messages entry below @@ -81,7 +81,7 @@ find_package(catkin REQUIRED COMPONENTS ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" ## * In this file (CMakeLists.txt): ## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) +## find_package(ros2 REQUIRED COMPONENTS ...) ## * uncomment the "generate_dynamic_reconfigure_options" section below ## and list every .cfg file to be processed @@ -92,15 +92,15 @@ find_package(catkin REQUIRED COMPONENTS # ) ################################### -## catkin specific configuration ## +## ros2 specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package +## The ros2_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDE_DIRS: uncomment this if your package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need +## CATKIN_DEPENDS: ros2_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( +ros2_package( # INCLUDE_DIRS include # LIBRARIES fusion2urdf # CATKIN_DEPENDS rospy @@ -115,7 +115,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( # include - ${catkin_INCLUDE_DIRS} + ${ros2_INCLUDE_DIRS} ) ## Declare a C++ library @@ -126,10 +126,10 @@ include_directories( ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ros2_EXPORTED_TARGETS}) ## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context +## With ros2_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/fusion2urdf_node.cpp) @@ -141,19 +141,19 @@ include_directories( ## Add cmake target dependencies of the executable ## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${ros2_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against # target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} +# ${ros2_LIBRARIES} # ) ############# ## Install ## ############# -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html +# all install targets should use ros2 DESTINATION variables +# See http://ros.org/doc/api/ros2/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination @@ -188,10 +188,10 @@ include_directories( ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_fusion2urdf.cpp) +# ros2_add_gtest(${PROJECT_NAME}-test test/test_fusion2urdf.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +# ros2_add_nosetests(test) diff --git a/soccer_description/bez2_description/package.xml b/soccer_description/bez2_description/package.xml index 636e56fee..4435cc66f 100644 --- a/soccer_description/bez2_description/package.xml +++ b/soccer_description/bez2_description/package.xml @@ -29,7 +29,7 @@ - + @@ -41,14 +41,14 @@ - + - catkin + ros2 rospy rospy rospy diff --git a/soccer_description/bez2_description/urdf/bez2.xacro b/soccer_description/bez2_description/urdf/bez2.xacro index 17132b4bd..4c044318a 100644 --- a/soccer_description/bez2_description/urdf/bez2.xacro +++ b/soccer_description/bez2_description/urdf/bez2.xacro @@ -4,7 +4,7 @@ - + diff --git a/soccer_hardware/soccer_firmware/.cproject b/soccer_hardware/soccer_firmware/.cproject index 15fab6946..ac26a060f 100644 --- a/soccer_hardware/soccer_firmware/.cproject +++ b/soccer_hardware/soccer_firmware/.cproject @@ -23,9 +23,9 @@