diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..3442bc76 --- /dev/null +++ b/.clang-format @@ -0,0 +1,4 @@ +--- +Language: Cpp +BasedOnStyle: Google +ColumnLimit: 120 diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs new file mode 100644 index 00000000..f3b53fc4 --- /dev/null +++ b/.git-blame-ignore-revs @@ -0,0 +1,2 @@ +# Add git commit hashes to ignore for blame +7032e318260581fa759df467ce6638333fb6b108 diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 00000000..bb684803 --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,16 @@ +name: Code style checks + +on: + pull_request: + push: + branches: [main] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v4 + - name: Install cppcheck + run: sudo apt install cppcheck -y + - uses: pre-commit/action@v3.0.0 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 00000000..561c3bd3 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,23 @@ +repos: +- repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.1.6 + hooks: + - id: ruff + args: + - "--fix" + - "--exit-non-zero-on-fix" +- repo: https://github.com/psf/black + rev: 23.11.0 + hooks: + - id: black +- repo: https://github.com/pocc/pre-commit-hooks + rev: v1.3.5 + hooks: + - id: clang-format + args: + - "-i" + - id: cppcheck + args: + - "--suppress=missingInclude" + - "--suppress=unmatchedSuppression" + - "--suppress=unusedFunction" diff --git a/bitbots_basler_camera/docs/conf.py b/bitbots_basler_camera/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_basler_camera/docs/conf.py +++ b/bitbots_basler_camera/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_basler_camera/setup.py b/bitbots_basler_camera/setup.py index 1abb0602..058104de 100644 --- a/bitbots_basler_camera/setup.py +++ b/bitbots_basler_camera/setup.py @@ -1,10 +1,11 @@ from distutils.core import setup + from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - #packages=['bitbots_basler_camera'], - #scripts=['bin/myscript'], - #package_dir={'': 'src'} + # packages=['bitbots_basler_camera'], + # scripts=['bin/myscript'], + # package_dir={'': 'src'} ) setup(**d) diff --git a/bitbots_bringup/docs/conf.py b/bitbots_bringup/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_bringup/docs/conf.py +++ b/bitbots_bringup/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_bringup/launch/rosbag_record.launch.py b/bitbots_bringup/launch/rosbag_record.launch.py index ccf84b64..3321281c 100644 --- a/bitbots_bringup/launch/rosbag_record.launch.py +++ b/bitbots_bringup/launch/rosbag_record.launch.py @@ -1,119 +1,115 @@ -from typing import List - import os - from datetime import datetime +from typing import List from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PathJoinSubstitution - TOPICS_TO_RECORD: List[str] = [ - '/animation', - '/audio/audio_info', - '/audio/audio', - '/ball_obstacle_active', - '/ball_position_relative_filtered', - '/ball_relative_filtered', - '/ball_relative_movement', - '/balls_relative', - '/camera/camera_info', - '/camera/image_to_record', - '/clock', - '/cmd_vel', - '/cop_l', - '/cop_r', - '/core/power_switch_status', - '/debug/approach_point', - '/debug/ball_twist', - '/debug/dsd/body_behavior', - '/debug/dsd/hcm', - '/debug/dsd/localization', - '/debug/used_ball', - '/debug/which_ball_is_used', - '/diagnostics', - '/diagnostics_agg', - '/DynamixelController/command', - '/field_boundary_relative', - '/game_controller_connected', - '/gamestate', - '/goal_pose', - '/head_mode', - '/imu_head/data', - '/imu/data_raw', - '/joint_states', - '/motion_odometry', - '/move_base/current_goal', - '/pose_with_covariance', - '/robot_state', - '/robots_relative', - '/robots_relative_filtered', - '/rosout', - '/server_time_clock', - '/speak', - '/strategy', - '/system_workload', - '/team_data', - '/tf', - '/tf_static', - '/time_to_ball', + "/animation", + "/audio/audio_info", + "/audio/audio", + "/ball_obstacle_active", + "/ball_position_relative_filtered", + "/ball_relative_filtered", + "/ball_relative_movement", + "/balls_relative", + "/camera/camera_info", + "/camera/image_to_record", + "/clock", + "/cmd_vel", + "/cop_l", + "/cop_r", + "/core/power_switch_status", + "/debug/approach_point", + "/debug/ball_twist", + "/debug/dsd/body_behavior", + "/debug/dsd/hcm", + "/debug/dsd/localization", + "/debug/used_ball", + "/debug/which_ball_is_used", + "/diagnostics", + "/diagnostics_agg", + "/DynamixelController/command", + "/field_boundary_relative", + "/game_controller_connected", + "/gamestate", + "/goal_pose", + "/head_mode", + "/imu_head/data", + "/imu/data_raw", + "/joint_states", + "/motion_odometry", + "/move_base/current_goal", + "/pose_with_covariance", + "/robot_state", + "/robots_relative", + "/robots_relative_filtered", + "/rosout", + "/server_time_clock", + "/speak", + "/strategy", + "/system_workload", + "/team_data", + "/tf", + "/tf_static", + "/time_to_ball", ] def generate_launch_arguments(): return [ DeclareLaunchArgument( - 'sim', - default_value='false', - description='true: Use simulation time', - choices=['true', 'false'] + "sim", default_value="false", description="true: Use simulation time", choices=["true", "false"] ), DeclareLaunchArgument( - 'max_image_frequency', - default_value='1.0', - description='Max frequency [hz] for recording images' + "max_image_frequency", default_value="1.0", description="Max frequency [hz] for recording images" ), DeclareLaunchArgument( - 'max_pointcloud_frequency', - default_value='1.0', - description='Max frequency [hz] for recording pointclouds' + "max_pointcloud_frequency", default_value="1.0", description="Max frequency [hz] for recording pointclouds" ), ] def generate_action(context): - robot_name = os.getenv('ROBOCUP_ROBOT_ID', default=os.getenv('ROBOT_NAME', default='unknown_robot')) + robot_name = os.getenv("ROBOCUP_ROBOT_ID", default=os.getenv("ROBOT_NAME", default="unknown_robot")) # Set output directory # ~/rosbags/ID__ - output_directory = PathJoinSubstitution([ - EnvironmentVariable('HOME'), - 'rosbags', - 'ID_' + robot_name + '_' + datetime.now().isoformat(timespec='seconds') - ]) + output_directory = PathJoinSubstitution( + [ + EnvironmentVariable("HOME"), + "rosbags", + "ID_" + robot_name + "_" + datetime.now().isoformat(timespec="seconds"), + ] + ) - sim_value = LaunchConfiguration('sim').perform(context) - sim_time = ['--use-sim-time'] if sim_value == 'true' else [] + sim_value = LaunchConfiguration("sim").perform(context) + sim_time = ["--use-sim-time"] if sim_value == "true" else [] main_process = ExecuteProcess( # Constructing the complete command cmd=[ # Main command to start recording ros2 bags - 'ros2', - 'bag', - 'record', - '-o', output_directory, - + "ros2", + "bag", + "record", + "-o", + output_directory, # Other options - '--node-name', 'ros2_bag_record', - '--include-hidden-topics', - '--include-unpublished-topics', - '--polling-interval', '1000', - ] + sim_time + TOPICS_TO_RECORD, - output='screen', - name='ros2_bag_record', - shell=True + "--node-name", + "ros2_bag_record", + "--include-hidden-topics", + "--include-unpublished-topics", + "--polling-interval", + "1000", + ] + + sim_time + + TOPICS_TO_RECORD, + output="screen", + name="ros2_bag_record", + shell=True, ) return [main_process] diff --git a/bitbots_bringup/setup.py b/bitbots_bringup/setup.py index 196127f8..d8fe7a2f 100644 --- a/bitbots_bringup/setup.py +++ b/bitbots_bringup/setup.py @@ -1,10 +1,6 @@ -import glob -import os - -from setuptools import find_packages from setuptools import setup -package_name = 'bitbots_bringup' +package_name = "bitbots_bringup" setup( @@ -12,9 +8,9 @@ packages=[], data_files=[], install_requires=[ - 'setuptools', + "setuptools", ], zip_safe=True, - keywords=['ROS'], - license='MIT', + keywords=["ROS"], + license="MIT", ) diff --git a/bitbots_ceiling_cam/docs/conf.py b/bitbots_ceiling_cam/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_ceiling_cam/docs/conf.py +++ b/bitbots_ceiling_cam/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_ceiling_cam/setup.py b/bitbots_ceiling_cam/setup.py index d13d1ec8..1aebf766 100644 --- a/bitbots_ceiling_cam/setup.py +++ b/bitbots_ceiling_cam/setup.py @@ -1,10 +1,11 @@ from distutils.core import setup + from catkin_pkg.python_setup import generate_distutils_setup d = generate_distutils_setup( - #packages=['bitbots_ceiling_cam'], - #scripts=['bin/myscript'], - #package_dir={'': 'src'} + # packages=['bitbots_ceiling_cam'], + # scripts=['bin/myscript'], + # package_dir={'': 'src'} ) setup(**d) diff --git a/bitbots_extrinsic_calibration/docs/conf.py b/bitbots_extrinsic_calibration/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_extrinsic_calibration/docs/conf.py +++ b/bitbots_extrinsic_calibration/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.h b/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp similarity index 99% rename from bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.h rename to bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp index fc9a8bc7..610cfd06 100644 --- a/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.h +++ b/bitbots_extrinsic_calibration/include/extrinsic_calibration/extrinsic_calibration.hpp @@ -1,12 +1,12 @@ -#include -#include +#include +#include +#include #include + #include -#include -#include -#include #include - +#include +#include #include using std::placeholders::_1; @@ -14,6 +14,7 @@ class ExtrinsicCalibrationBroadcaster : public rclcpp::Node { public: ExtrinsicCalibrationBroadcaster(); void step(); + private: OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; std::unique_ptr broadcaster_; diff --git a/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp b/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp index 2e3df046..82342cf2 100644 --- a/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp +++ b/bitbots_extrinsic_calibration/src/extrinsic_calibration.cpp @@ -1,7 +1,6 @@ -#include +#include ExtrinsicCalibrationBroadcaster::ExtrinsicCalibrationBroadcaster() : Node("bitbots_extrinsic_calibration") { - broadcaster_ = std::make_unique(this); this->declare_parameter("parent_frame", "camera"); @@ -13,49 +12,45 @@ ExtrinsicCalibrationBroadcaster::ExtrinsicCalibrationBroadcaster() : Node("bitbo auto parameters = this->get_parameters(this->list_parameters({}, 10).names); ExtrinsicCalibrationBroadcaster::onSetParameters(parameters); - param_callback_handle_ = this->add_on_set_parameters_callback( - std::bind(&ExtrinsicCalibrationBroadcaster::onSetParameters, this, _1)); + param_callback_handle_ = + this->add_on_set_parameters_callback(std::bind(&ExtrinsicCalibrationBroadcaster::onSetParameters, this, _1)); } rcl_interfaces::msg::SetParametersResult ExtrinsicCalibrationBroadcaster::onSetParameters( - const std::vector ¶meters) - { - - for (const auto ¶meter: parameters) { - if (parameter.get_name() == "offset_x") { - offset_x_ = parameter.as_double(); - } else if (parameter.get_name() == "offset_y") { - offset_y_ = parameter.as_double(); - } else if (parameter.get_name() == "offset_z") { - offset_z_ = parameter.as_double(); - } else if(parameter.get_name() == "parent_frame") { - parent_frame_ = parameter.as_string(); - } else if(parameter.get_name() == "child_frame") { - child_frame_ = parameter.as_string(); - } else { - RCLCPP_DEBUG(this->get_logger(), "Unknown parameter: %s", parameter.get_name().c_str()); - } + const std::vector ¶meters) { + for (const auto ¶meter : parameters) { + if (parameter.get_name() == "offset_x") { + offset_x_ = parameter.as_double(); + } else if (parameter.get_name() == "offset_y") { + offset_y_ = parameter.as_double(); + } else if (parameter.get_name() == "offset_z") { + offset_z_ = parameter.as_double(); + } else if (parameter.get_name() == "parent_frame") { + parent_frame_ = parameter.as_string(); + } else if (parameter.get_name() == "child_frame") { + child_frame_ = parameter.as_string(); + } else { + RCLCPP_DEBUG(this->get_logger(), "Unknown parameter: %s", parameter.get_name().c_str()); } + } - auto base_quat = rot_conv::QuatFromEuler(-1.5708, 0.0, -1.5708); - auto offset_quat = rot_conv::QuatFromEuler(offset_z_, offset_y_, offset_x_); - - auto final_quat = offset_quat * base_quat; + auto base_quat = rot_conv::QuatFromEuler(-1.5708, 0.0, -1.5708); + auto offset_quat = rot_conv::QuatFromEuler(offset_z_, offset_y_, offset_x_); - transform_.rotation.x = final_quat.x(); - transform_.rotation.y = final_quat.y(); - transform_.rotation.z = final_quat.z(); - transform_.rotation.w = final_quat.w(); + auto final_quat = offset_quat * base_quat; - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; + transform_.rotation.x = final_quat.x(); + transform_.rotation.y = final_quat.y(); + transform_.rotation.z = final_quat.z(); + transform_.rotation.w = final_quat.w(); - return result; - } + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; +} void ExtrinsicCalibrationBroadcaster::step() { - auto node_pointer = this->shared_from_this(); rclcpp::Time now = this->now(); geometry_msgs::msg::TransformStamped tf; @@ -71,12 +66,11 @@ int main(int argc, char **argv) { auto node = std::make_shared(); - rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer( - node, node->get_clock(), rclcpp::Duration(0, 1e7), [node]() -> void {node->step();}); + rclcpp::TimerBase::SharedPtr timer = + rclcpp::create_timer(node, node->get_clock(), rclcpp::Duration(0, 1e7), [node]() -> void { node->step(); }); rclcpp::experimental::executors::EventsExecutor exec; exec.add_node(node); exec.spin(); rclcpp::shutdown(); } - diff --git a/bitbots_ipm/setup.py b/bitbots_ipm/setup.py index bd548682..40eb022b 100644 --- a/bitbots_ipm/setup.py +++ b/bitbots_ipm/setup.py @@ -1,7 +1,7 @@ from distutils.core import setup + from catkin_pkg.python_setup import generate_distutils_setup -d = generate_distutils_setup( -) +d = generate_distutils_setup() setup(**d) diff --git a/bitbots_robot_description/docs/conf.py b/bitbots_robot_description/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_robot_description/docs/conf.py +++ b/bitbots_robot_description/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_robot_description/launch/move_group.py b/bitbots_robot_description/launch/move_group.py index 995a677e..d36a0f5a 100644 --- a/bitbots_robot_description/launch/move_group.py +++ b/bitbots_robot_description/launch/move_group.py @@ -4,10 +4,9 @@ from ament_index_python import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import Command, LaunchConfiguration, TextSubstitution from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue -from launch.substitutions import Command, TextSubstitution, LaunchConfiguration -from launch.substitutions import TextSubstitution def load_file(package_name, file_path): @@ -15,9 +14,9 @@ def load_file(package_name, file_path): absolute_file_path = os.path.join(package_path, file_path) try: - with open(absolute_file_path, "r") as file: + with open(absolute_file_path) as file: return file.read() - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except OSError: # parent of IOError, OSError *and* WindowsError where available return None @@ -26,9 +25,9 @@ def load_yaml(package_name, file_path): absolute_file_path = os.path.join(package_path, file_path) try: - with open(absolute_file_path, "r") as file: + with open(absolute_file_path) as file: return yaml.safe_load(file) - except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + except OSError: # parent of IOError, OSError *and* WindowsError where available return None @@ -38,16 +37,27 @@ def launch_setup(context, *args, **kwargs): sim_ns = LaunchConfiguration("sim_ns") robot_type = LaunchConfiguration("robot_type") - robot_description = ParameterValue(Command(['xacro ', - os.path.join( - get_package_share_directory(f"{robot_type.perform(context)}_description"), - "urdf", - "robot.urdf", - ), " use_fake_walk:=", fake_walk, " sim_ns:=", sim_ns]), - value_type=str) + robot_description = ParameterValue( + Command( + [ + "xacro ", + os.path.join( + get_package_share_directory(f"{robot_type.perform(context)}_description"), + "urdf", + "robot.urdf", + ), + " use_fake_walk:=", + fake_walk, + " sim_ns:=", + sim_ns, + ] + ), + value_type=str, + ) - robot_description_semantic_config = load_file(f"{robot_type.perform(context)}_moveit_config", - f"config/{robot_type.perform(context)}.srdf") + robot_description_semantic_config = load_file( + f"{robot_type.perform(context)}_moveit_config", f"config/{robot_type.perform(context)}.srdf" + ) kinematics_yaml = load_yaml(f"{robot_type.perform(context)}_moveit_config", "config/kinematics.yaml") ompl_planning_pipeline_config = { @@ -57,9 +67,7 @@ def launch_setup(context, *args, **kwargs): "start_state_max_bounds_error": 0.1, } } - ompl_planning_yaml = load_yaml( - f"{robot_type.perform(context)}_moveit_config", "config/ompl_planning.yaml" - ) + ompl_planning_yaml = load_yaml(f"{robot_type.perform(context)}_moveit_config", "config/ompl_planning.yaml") ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) # Trajectory Execution Functionality moveit_simple_controllers_yaml = load_yaml( @@ -102,49 +110,46 @@ def launch_setup(context, *args, **kwargs): if planning_scene_monitor_parameters is None: print("### WARNING: planning_scene_monitor_parameters is None") - - rsp_node = Node(package='robot_state_publisher', - executable='robot_state_publisher', - respawn=True, - output='screen', - parameters=[{ - 'robot_description': robot_description, - 'publish_frequency': 100.0, - 'use_sim_time': sim - }], - arguments=['--ros-args', '--log-level', 'WARN'] - ) + rsp_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + respawn=True, + output="screen", + parameters=[{"robot_description": robot_description, "publish_frequency": 100.0, "use_sim_time": sim}], + arguments=["--ros-args", "--log-level", "WARN"], + ) move_group_node = Node( - package='moveit_ros_move_group', - executable='move_group', - output='screen', + package="moveit_ros_move_group", + executable="move_group", + output="screen", # hacky merging dicts parameters=[ - { - 'robot_description': robot_description, - 'robot_description_semantic': robot_description_semantic_config, - 'robot_description_kinematics': kinematics_yaml, - 'publish_robot_description_semantic': True, - 'use_sim_time': sim, - 'octomap_resolution': 0.01, + { + "robot_description": robot_description, + "robot_description_semantic": robot_description_semantic_config, + "robot_description_kinematics": kinematics_yaml, + "publish_robot_description_semantic": True, + "use_sim_time": sim, + "octomap_resolution": 0.01, }, ompl_planning_pipeline_config, trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, - sensor_config + sensor_config, ], - arguments=['--ros-args', '--log-level', 'WARN'] - ) # todo joint limits + arguments=["--ros-args", "--log-level", "WARN"], + ) # todo joint limits return [move_group_node, rsp_node] def generate_launch_description(): - sim_arg = DeclareLaunchArgument('sim', default_value='False') - fake_walk_arg = DeclareLaunchArgument('fake_walk', default_value='False') - sim_ns_arg = DeclareLaunchArgument('sim_ns', default_value=TextSubstitution(text='/')) - robot_type_arg = DeclareLaunchArgument('robot_type', default_value=TextSubstitution(text='wolfgang')) + sim_arg = DeclareLaunchArgument("sim", default_value="False") + fake_walk_arg = DeclareLaunchArgument("fake_walk", default_value="False") + sim_ns_arg = DeclareLaunchArgument("sim_ns", default_value=TextSubstitution(text="/")) + robot_type_arg = DeclareLaunchArgument("robot_type", default_value=TextSubstitution(text="wolfgang")) - return LaunchDescription([sim_arg, fake_walk_arg, sim_ns_arg, robot_type_arg, - OpaqueFunction(function=launch_setup)]) + return LaunchDescription( + [sim_arg, fake_walk_arg, sim_ns_arg, robot_type_arg, OpaqueFunction(function=launch_setup)] + ) diff --git a/bitbots_teleop/bitbots_teleop/joy_node.py b/bitbots_teleop/bitbots_teleop/joy_node.py index 1f7c768d..b61e7c02 100755 --- a/bitbots_teleop/bitbots_teleop/joy_node.py +++ b/bitbots_teleop/bitbots_teleop/joy_node.py @@ -1,92 +1,120 @@ #!/usr/bin/env python3 -# -*- coding:utf-8 -*- -import threading -import rclpy -from rclpy.duration import Duration -from rclpy.node import Node -from rclpy.action import ActionClient import copy -from bitbots_msgs.action import PlayAnimation -from bitbots_msgs.msg import Audio -from bitbots_msgs.msg import JointCommand, HeadMode +import rclpy +from bitbots_msgs.msg import Audio, HeadMode, JointCommand from geometry_msgs.msg import Twist +from rclpy.duration import Duration +from rclpy.node import Node from sensor_msgs.msg import Joy class JoyNode(Node): - """ This node controls the roboter via Gamepad. - """ + """This node controls the roboter via Gamepad.""" - #TODO read max values from config + # TODO read max values from config def __init__(self): - super().__init__('joy_node') + super().__init__("joy_node") - self.declare_parameter('type', "noname") - self.declare_parameter('head', False) + self.declare_parameter("type", "noname") + self.declare_parameter("head", False) for controller_type in ["noname", "xbox"]: - self.declare_parameter(f'{controller_type}.walking.gain_x', 0.0) - self.declare_parameter(f'{controller_type}.walking.stick_x', 0) - self.declare_parameter(f'{controller_type}.walking.gain_y', 0.0) - self.declare_parameter(f'{controller_type}.walking.stick_y', 0) - self.declare_parameter(f'{controller_type}.walking.stick_left', 0) - self.declare_parameter(f'{controller_type}.walking.stick_right', 0) - self.declare_parameter(f'{controller_type}.walking.duo_turn', False) - self.declare_parameter(f'{controller_type}.walking.gain_turn', 0.0) - self.declare_parameter(f'{controller_type}.walking.btn_full_stop', 0) - self.declare_parameter(f'{controller_type}.head.gain_tilt', 0.0) - self.declare_parameter(f'{controller_type}.head.stick_tilt', 0) - self.declare_parameter(f'{controller_type}.head.gain_pan', 0.0) - self.declare_parameter(f'{controller_type}.head.stick_pan', 0) - self.declare_parameter(f'{controller_type}.kick.btn_left', 0) - self.declare_parameter(f'{controller_type}.kick.btn_right', 0) - self.declare_parameter(f'{controller_type}.misc.btn_cheering', 0) - self.declare_parameter(f'{controller_type}.misc.btn_say', 0) + self.declare_parameter(f"{controller_type}.walking.gain_x", 0.0) + self.declare_parameter(f"{controller_type}.walking.stick_x", 0) + self.declare_parameter(f"{controller_type}.walking.gain_y", 0.0) + self.declare_parameter(f"{controller_type}.walking.stick_y", 0) + self.declare_parameter(f"{controller_type}.walking.stick_left", 0) + self.declare_parameter(f"{controller_type}.walking.stick_right", 0) + self.declare_parameter(f"{controller_type}.walking.duo_turn", False) + self.declare_parameter(f"{controller_type}.walking.gain_turn", 0.0) + self.declare_parameter(f"{controller_type}.walking.btn_full_stop", 0) + self.declare_parameter(f"{controller_type}.head.gain_tilt", 0.0) + self.declare_parameter(f"{controller_type}.head.stick_tilt", 0) + self.declare_parameter(f"{controller_type}.head.gain_pan", 0.0) + self.declare_parameter(f"{controller_type}.head.stick_pan", 0) + self.declare_parameter(f"{controller_type}.kick.btn_left", 0) + self.declare_parameter(f"{controller_type}.kick.btn_right", 0) + self.declare_parameter(f"{controller_type}.misc.btn_cheering", 0) + self.declare_parameter(f"{controller_type}.misc.btn_say", 0) selected_controller_type = self.get_parameter("type").get_parameter_value().string_value self.config = {} self.config["walking"] = { - "gain_x": self.get_parameter(f"{selected_controller_type}.walking.gain_x").get_parameter_value().double_value, - "stick_x": self.get_parameter(f"{selected_controller_type}.walking.stick_x").get_parameter_value().integer_value, - "gain_y": self.get_parameter(f"{selected_controller_type}.walking.gain_y").get_parameter_value().double_value, - "stick_y": self.get_parameter(f"{selected_controller_type}.walking.stick_y").get_parameter_value().integer_value, - "stick_right": self.get_parameter(f"{selected_controller_type}.walking.stick_right").get_parameter_value().integer_value, - "stick_left": self.get_parameter(f"{selected_controller_type}.walking.stick_left").get_parameter_value().integer_value, - "duo_turn": self.get_parameter(f"{selected_controller_type}.walking.duo_turn").get_parameter_value().bool_value, - "gain_turn": self.get_parameter(f"{selected_controller_type}.walking.gain_turn").get_parameter_value().double_value, - "btn_full_stop": self.get_parameter(f"{selected_controller_type}.walking.btn_full_stop").get_parameter_value().integer_value, + "gain_x": self.get_parameter(f"{selected_controller_type}.walking.gain_x") + .get_parameter_value() + .double_value, + "stick_x": self.get_parameter(f"{selected_controller_type}.walking.stick_x") + .get_parameter_value() + .integer_value, + "gain_y": self.get_parameter(f"{selected_controller_type}.walking.gain_y") + .get_parameter_value() + .double_value, + "stick_y": self.get_parameter(f"{selected_controller_type}.walking.stick_y") + .get_parameter_value() + .integer_value, + "stick_right": self.get_parameter(f"{selected_controller_type}.walking.stick_right") + .get_parameter_value() + .integer_value, + "stick_left": self.get_parameter(f"{selected_controller_type}.walking.stick_left") + .get_parameter_value() + .integer_value, + "duo_turn": self.get_parameter(f"{selected_controller_type}.walking.duo_turn") + .get_parameter_value() + .bool_value, + "gain_turn": self.get_parameter(f"{selected_controller_type}.walking.gain_turn") + .get_parameter_value() + .double_value, + "btn_full_stop": self.get_parameter(f"{selected_controller_type}.walking.btn_full_stop") + .get_parameter_value() + .integer_value, } self.config["head"] = { - "gain_tilt": self.get_parameter(f"{selected_controller_type}.head.gain_tilt").get_parameter_value().double_value, - "stick_tilt": self.get_parameter(f"{selected_controller_type}.head.stick_tilt").get_parameter_value().integer_value, - "gain_pan": self.get_parameter(f"{selected_controller_type}.head.gain_pan").get_parameter_value().double_value, - "stick_pan": self.get_parameter(f"{selected_controller_type}.head.stick_pan").get_parameter_value().integer_value, + "gain_tilt": self.get_parameter(f"{selected_controller_type}.head.gain_tilt") + .get_parameter_value() + .double_value, + "stick_tilt": self.get_parameter(f"{selected_controller_type}.head.stick_tilt") + .get_parameter_value() + .integer_value, + "gain_pan": self.get_parameter(f"{selected_controller_type}.head.gain_pan") + .get_parameter_value() + .double_value, + "stick_pan": self.get_parameter(f"{selected_controller_type}.head.stick_pan") + .get_parameter_value() + .integer_value, } self.config["kick"] = { - "btn_left": self.get_parameter(f"{selected_controller_type}.kick.btn_left").get_parameter_value().integer_value, - "btn_right": self.get_parameter(f"{selected_controller_type}.kick.btn_right").get_parameter_value().integer_value, + "btn_left": self.get_parameter(f"{selected_controller_type}.kick.btn_left") + .get_parameter_value() + .integer_value, + "btn_right": self.get_parameter(f"{selected_controller_type}.kick.btn_right") + .get_parameter_value() + .integer_value, } self.config["misc"] = { - "btn_cheering": self.get_parameter(f"{selected_controller_type}.misc.btn_cheering").get_parameter_value().integer_value, - "btn_say": self.get_parameter(f"{selected_controller_type}.misc.btn_say").get_parameter_value().integer_value, + "btn_cheering": self.get_parameter(f"{selected_controller_type}.misc.btn_cheering") + .get_parameter_value() + .integer_value, + "btn_say": self.get_parameter(f"{selected_controller_type}.misc.btn_say") + .get_parameter_value() + .integer_value, } print(self.config) # --- Initialize Topics --- - self.create_subscription(Joy,"joy", self.joy_cb, 1) - self.speak_pub = self.create_publisher(Audio, 'speak', 1) + self.create_subscription(Joy, "joy", self.joy_cb, 1) + self.speak_pub = self.create_publisher(Audio, "speak", 1) self.speak_msg = Audio() self.speak_msg.priority = 1 - self.walk_publisher = self.create_publisher(Twist, 'cmd_vel', 1) + self.walk_publisher = self.create_publisher(Twist, "cmd_vel", 1) self.walk_msg = Twist() self.last_walk_msg = Twist() @@ -111,19 +139,18 @@ def __init__(self): self.head_modes = HeadMode() # --- init animation action --- - #self.anim_client = ActionClient(self, PlayAnimationAction, 'animation') - #self.anim_goal = PlayAnimationAction.Goal() - #self.anim_goal.hcm = False + # self.anim_client = ActionClient(self, PlayAnimationAction, 'animation') + # self.anim_goal = PlayAnimationAction.Goal() + # self.anim_goal.hcm = False - #first_try = self.anim_client.wait_for_server(Duration(seconds=1)) + # first_try = self.anim_client.wait_for_server(Duration(seconds=1)) - #if not first_try: + # if not first_try: # self.get_logger().error( # "Animation Action Server not running! Teleop can not work without animation action server. " # "Will now wait until server is accessible!") - #self.anim_client.wait_for_server() - #self.get_logger().warn("Animation server running, will go on.") - + # self.anim_client.wait_for_server() + # self.get_logger().warn("Animation server running, will go on.") def play_animation(self, name): return @@ -150,20 +177,18 @@ def denormalize_joy(self, gain, axis, msg: Joy, deadzone=0.0): def joy_cb(self, msg: Joy): # forward and sideward walking with left joystick - self.walk_msg.linear.x = float(self.denormalize_joy( - self.config['walking']['gain_x'], - self.config['walking']['stick_x'], - msg, 0.01)) - self.walk_msg.linear.y = float(self.denormalize_joy( - self.config['walking']['gain_y'], - self.config['walking']['stick_y'], - msg, 0.01)) + self.walk_msg.linear.x = float( + self.denormalize_joy(self.config["walking"]["gain_x"], self.config["walking"]["stick_x"], msg, 0.01) + ) + self.walk_msg.linear.y = float( + self.denormalize_joy(self.config["walking"]["gain_y"], self.config["walking"]["stick_y"], msg, 0.01) + ) # angular walking with shoulder buttons - turn = msg.axes[self.config['walking']['stick_left']] - if self.config['walking']['duo_turn']: - turn -= msg.axes[self.config['walking']['stick_right']] - turn *= self.config['walking']['gain_turn'] + turn = msg.axes[self.config["walking"]["stick_left"]] + if self.config["walking"]["duo_turn"]: + turn -= msg.axes[self.config["walking"]["stick_right"]] + turn *= self.config["walking"]["gain_turn"] if turn != 0: self.walk_msg.angular.z = float(turn) @@ -171,7 +196,7 @@ def joy_cb(self, msg: Joy): self.walk_msg.angular.z = 0.0 # Perform full stop (finish step but do not move afterwards) - if msg.buttons[self.config['walking']["btn_full_stop"]] == 1: + if msg.buttons[self.config["walking"]["btn_full_stop"]] == 1: self.walk_msg.linear.x = 0.0 self.walk_msg.linear.y = 0.0 self.walk_msg.linear.z = 0.0 @@ -184,39 +209,36 @@ def joy_cb(self, msg: Joy): self.walk_msg.angular.x = 0.0 # only publish changes - if self.walk_msg.linear != self.last_walk_msg.linear or \ - self.walk_msg.angular.z != self.last_walk_msg.angular.z: + if self.walk_msg.linear != self.last_walk_msg.linear or self.walk_msg.angular.z != self.last_walk_msg.angular.z: self.walk_publisher.publish(self.walk_msg) self.last_walk_msg = copy.deepcopy(self.walk_msg) # head movement with right joystick if self.get_parameter("head").get_parameter_value().bool_value: - pan_goal = float(self.denormalize_joy( - self.config['head']['gain_pan'], - self.config['head']['stick_pan'], - msg, -1)) - tilt_goal = float(self.denormalize_joy( - self.config['head']['gain_tilt'], - self.config['head']['stick_tilt'], - msg, -1)) + pan_goal = float( + self.denormalize_joy(self.config["head"]["gain_pan"], self.config["head"]["stick_pan"], msg, -1) + ) + tilt_goal = float( + self.denormalize_joy(self.config["head"]["gain_tilt"], self.config["head"]["stick_tilt"], msg, -1) + ) self.head_msg.joint_names = ["HeadPan", "HeadTilt"] self.head_msg.positions = [pan_goal, tilt_goal] self.head_pub.publish(self.head_msg) - if msg.buttons[self.config['kick']["btn_left"]]: + if msg.buttons[self.config["kick"]["btn_left"]]: self.play_animation("kick_left") - elif msg.buttons[self.config['kick']["btn_right"]]: + elif msg.buttons[self.config["kick"]["btn_right"]]: self.play_animation("kick_right") - elif msg.buttons[self.config['misc']["btn_cheering"]]: + elif msg.buttons[self.config["misc"]["btn_cheering"]]: self.play_animation("cheering") - elif msg.buttons[self.config['misc']["btn_say"]]: + elif msg.buttons[self.config["misc"]["btn_say"]]: self.send_text("Goal!") + def main(): rclpy.init(args=None) node = JoyNode() rclpy.spin(node) node.destroy_node() rclpy.shutdown() - diff --git a/bitbots_teleop/scripts/teleop_keyboard.py b/bitbots_teleop/scripts/teleop_keyboard.py index 51efd27f..746c4451 100755 --- a/bitbots_teleop/scripts/teleop_keyboard.py +++ b/bitbots_teleop/scripts/teleop_keyboard.py @@ -3,24 +3,23 @@ # This script was based on the teleop_twist_keyboard package # original code can be found at https://github.com/ros-teleop/teleop_twist_keyboard import os +import select +import sys +import termios import threading +import tty import rclpy -from rclpy.node import Node - -from geometry_msgs.msg import Twist -from sensor_msgs.msg import JointState +from bitbots_msgs.action import Dynup, Kick from bitbots_msgs.msg import JointCommand -from bitbots_utils.transforms import quat_from_yaw - -import sys, select, termios, tty +from geometry_msgs.msg import Point, Twist, Vector3 from rclpy.action import ActionClient -from bitbots_msgs.action import Kick, Dynup -from geometry_msgs.msg import Point, Vector3 +from rclpy.node import Node +from sensor_msgs.msg import JointState from std_msgs.msg import Bool from std_srvs.srv import Empty - +from bitbots_utils.transforms import quat_from_yaw msg = """ BitBots Teleop @@ -57,51 +56,49 @@ """ -moveBindings = { - 'w': (1, 0, 0), - 's': (-1, 0, 0), - 'a': (0, 1, 0), - 'd': (0, -1, 0), - 'q': (0, 0, 1), - 'e': (0, 0, -1), - 'W': (10, 0, 0), - 'S': (-10, 0, 0), - 'A': (0, 10, 0), - 'D': (0, -10, 0), - 'Q': (0, 0, 10), - 'E': (0, 0, -10), +move_bindings = { + "w": (1, 0, 0), + "s": (-1, 0, 0), + "a": (0, 1, 0), + "d": (0, -1, 0), + "q": (0, 0, 1), + "e": (0, 0, -1), + "W": (10, 0, 0), + "S": (-10, 0, 0), + "A": (0, 10, 0), + "D": (0, -10, 0), + "Q": (0, 0, 10), + "E": (0, 0, -10), } -headBindings = { - 'u': (1, 1), - 'i': (1, 0), - 'o': (1, -1), - 'j': (0, 1), - 'l': (0, -1), - 'm': (-1, 1), - ',': (-1, 0), - '.': (-1, -1), - 'U': (10, 10), - 'I': (10, 0), - 'O': (10, -10), - 'J': (0, 10), - 'L': (0, -10), - 'M': (-10, 10), - ';': (-10, 0), - ':': (-10, -10) +head_bindings = { + "u": (1, 1), + "i": (1, 0), + "o": (1, -1), + "j": (0, 1), + "l": (0, -1), + "m": (-1, 1), + ",": (-1, 0), + ".": (-1, -1), + "U": (10, 10), + "I": (10, 0), + "O": (10, -10), + "J": (0, 10), + "L": (0, -10), + "M": (-10, 10), + ";": (-10, 0), + ":": (-10, -10), } class TeleopKeyboard(Node): - def __init__(self): # create node super().__init__("TeleopKeyboard") self.settings = termios.tcgetattr(sys.stdin) - # Walking Part - self.pub = self.create_publisher(Twist, 'cmd_vel', 1) + self.pub = self.create_publisher(Twist, "cmd_vel", 1) self.head_pan_pos = 0 self.head_tilt_pos = 0 @@ -126,7 +123,7 @@ def __init__(self): self.head_msg.max_currents = [-1.0] * 2 self.head_msg.velocities = [5.0] * 2 self.head_msg.accelerations = [40.0] * 2 - self.head_msg.joint_names = ['HeadPan', 'HeadTilt'] + self.head_msg.joint_names = ["HeadPan", "HeadTilt"] self.head_msg.positions = [0.0] * 2 self.head_pan_step = 0.05 @@ -141,24 +138,22 @@ def __init__(self): self.frame_prefix = "" if os.environ.get("ROS_NAMESPACE") is None else os.environ.get("ROS_NAMESPACE") + "/" - self.dynup_client = ActionClient(self, Dynup, 'dynup') + self.dynup_client = ActionClient(self, Dynup, "dynup") if not self.dynup_client.wait_for_server(timeout_sec=5.0): - self.get_logger().error('Dynup action server not available after waiting 5 seconds') + self.get_logger().error("Dynup action server not available after waiting 5 seconds") - self.kick_client = ActionClient(self, Kick, 'dynamic_kick') + self.kick_client = ActionClient(self, Kick, "dynamic_kick") if not self.kick_client.wait_for_server(timeout_sec=5.0): - self.get_logger().error('Kick action server not available after waiting 5 seconds') + self.get_logger().error("Kick action server not available after waiting 5 seconds") - - - def getKey(self): + def get_key(self): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) key = sys.stdin.read(1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) return key - - def getWalkready(self): + + def get_walkready(self): goal = Dynup.Goal() goal.direction = "walkready" result: Dynup.Result = self.dynup_client.send_goal(goal).result @@ -183,41 +178,41 @@ def joint_state_cb(self, msg): def loop(self): try: while True: - key = self.getKey() - if key in moveBindings.keys(): - self.x += moveBindings[key][0] * self.x_speed_step + key = self.get_key() + if key in move_bindings.keys(): + self.x += move_bindings[key][0] * self.x_speed_step self.x = round(self.x, 2) - self.y += moveBindings[key][1] * self.y_speed_step + self.y += move_bindings[key][1] * self.y_speed_step self.y = round(self.y, 2) - self.th += moveBindings[key][2] * self.turn_speed_step + self.th += move_bindings[key][2] * self.turn_speed_step self.th = round(self.th, 2) self.a_x = 0 - elif key in headBindings.keys(): - self.head_msg.positions[0] = self.head_pan_pos + headBindings[key][1] * self.head_pan_step - self.head_msg.positions[1] = self.head_tilt_pos + headBindings[key][0] * self.head_tilt_step + elif key in head_bindings.keys(): + self.head_msg.positions[0] = self.head_pan_pos + head_bindings[key][1] * self.head_pan_step + self.head_msg.positions[1] = self.head_tilt_pos + head_bindings[key][0] * self.head_tilt_step self.head_pub.publish(self.head_msg) - elif key == 'k' or key == 'K': + elif key == "k" or key == "K": # put head back in init self.head_msg.positions[0] = 0 self.head_msg.positions[1] = 0 self.head_pub.publish(self.head_msg) - elif key == 'y': + elif key == "y": # kick left forward self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0.1, 0)) - elif key == '<': + elif key == "<": # kick left side ball left self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0.1, -1.57)) - elif key == '>': + elif key == ">": # kick left side ball center self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, -1.57)) - elif key == 'c': + elif key == "c": # kick right forward self.kick_client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 0)) - elif key == 'v': + elif key == "v": # kick right side ball right self.kick_client.send_goal_async(self.generate_kick_goal(0.2, -0.1, 1.57)) - elif key == 'V': + elif key == "V": # kick right side ball center self.kick_client.send_goal_async(self.generate_kick_goal(0.2, 0, 1.57)) elif key == "x": @@ -238,28 +233,28 @@ def loop(self): elif key == "N": # kick right backwards self.kick_client.send_goal_async(self.generate_kick_goal(0, -0.14, 1.57)) - elif key == 'Y': + elif key == "Y": # kick left walk self.walk_kick_pub.publish(Bool(data=False)) - elif key == 'C': + elif key == "C": # kick right walk self.walk_kick_pub.publish(Bool(data=True)) - elif key == 'F': + elif key == "F": # play walkready animation - self.getWalkready() - elif key == 'r': + self.get_walkready() + elif key == "r": # reset robot in sim try: self.reset_robot.call_async(Empty.Request()) - except: + except Exception: pass - elif key == 'R': + elif key == "R": # reset ball in sim try: self.reset_ball.call_async(Empty.Request()) - except: + except Exception: pass - elif key == 'f': + elif key == "f": # complete walk stop self.x = 0 self.y = 0 @@ -272,7 +267,7 @@ def loop(self): self.z = 0 self.a_x = 0 self.th = 0 - if (key == '\x03'): + if key == "\x03": self.a_x = -1 break @@ -285,8 +280,7 @@ def loop(self): sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A") sys.stdout.write("\x1b[A") - print( - f"x: {self.x} \ny: {self.y} \nturn: {self.th} \n\n") + print(f"x: {self.x} \ny: {self.y} \nturn: {self.th} \n\n") except Exception as e: print(e) diff --git a/bitbots_teleop/setup.py b/bitbots_teleop/setup.py index a8f379d7..0e1db432 100644 --- a/bitbots_teleop/setup.py +++ b/bitbots_teleop/setup.py @@ -1,37 +1,30 @@ import glob -import os -from setuptools import find_packages -from setuptools import setup +from setuptools import find_packages, setup -package_name = 'bitbots_teleop' +package_name = "bitbots_teleop" setup( name=package_name, - packages=find_packages(exclude=['test']), + packages=find_packages(exclude=["test"]), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + "/config", - glob.glob('config/*.yaml')), - ('share/' + package_name + '/launch', - glob.glob('launch/*.launch')), - ], - scripts=[ - 'scripts/teleop_keyboard.py' + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/config", glob.glob("config/*.yaml")), + ("share/" + package_name + "/launch", glob.glob("launch/*.launch")), ], + scripts=["scripts/teleop_keyboard.py"], install_requires=[ - 'launch', - 'setuptools', + "launch", + "setuptools", ], zip_safe=True, - keywords=['ROS'], - license='MIT', + keywords=["ROS"], + license="MIT", entry_points={ - 'console_scripts': [ - 'joy_node = bitbots_teleop.joy_node:main', + "console_scripts": [ + "joy_node = bitbots_teleop.joy_node:main", ], - } + }, ) diff --git a/bitbots_tf_listener/bitbots_tf_listener/__init__.py b/bitbots_tf_listener/bitbots_tf_listener/__init__.py index 42c5b540..6c1d6364 100644 --- a/bitbots_tf_listener/bitbots_tf_listener/__init__.py +++ b/bitbots_tf_listener/bitbots_tf_listener/__init__.py @@ -1,5 +1,6 @@ -from rclpy.serialization import deserialize_message from geometry_msgs.msg import TransformStamped +from rclpy.serialization import deserialize_message + from bitbots_tf_listener.cpp_wrapper import TransformListener as TransformListener_ diff --git a/bitbots_tf_listener/setup.py b/bitbots_tf_listener/setup.py index 0c2f3040..d2692a96 100644 --- a/bitbots_tf_listener/setup.py +++ b/bitbots_tf_listener/setup.py @@ -1,10 +1,9 @@ -from setuptools import setup, Extension -import os +from setuptools import setup setup( - name='bitbots_tf_listener', - packages=['bitbots_tf_listener'], + name="bitbots_tf_listener", + packages=["bitbots_tf_listener"], zip_safe=True, - keywords=['ROS'], - license='MIT', + keywords=["ROS"], + license="MIT", ) diff --git a/bitbots_tf_listener/src/bitbots_tf_listener.cpp b/bitbots_tf_listener/src/bitbots_tf_listener.cpp index aaf72df6..ba0d9e25 100644 --- a/bitbots_tf_listener/src/bitbots_tf_listener.cpp +++ b/bitbots_tf_listener/src/bitbots_tf_listener.cpp @@ -1,17 +1,17 @@ +#include #include #include -#include + +#include #include #include #include -#include - #include namespace py = pybind11; class TransformListener { -public: + public: TransformListener(py::object node, py::object py_wrapper) { // initialize rclcpp if not already done if (!rclcpp::contexts::get_global_default_context()->is_valid()) { @@ -19,7 +19,7 @@ class TransformListener { } // get node name from python node object - rcl_node_t *node_handle = (rcl_node_t*) node.attr("handle").attr("pointer").cast(); + rcl_node_t *node_handle = (rcl_node_t *)node.attr("handle").attr("pointer").cast(); const char *node_name = rcl_node_get_name(node_handle); // create node with name _tf_listener node_ = std::make_shared((std::string(node_name) + "_tf_listener").c_str()); @@ -48,9 +48,7 @@ class TransformListener { } // callbacks - void tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { - common_callback(msg, set_transform_); - } + void tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { common_callback(msg, set_transform_); } void tf_static_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { common_callback(msg, set_transform_static_); @@ -58,7 +56,8 @@ class TransformListener { // function with common code (serialization) from both callbacks void common_callback(const tf2_msgs::msg::TFMessage::SharedPtr &msg, py::object set_transform) { - // the message is serialized to python to be able to transfer it to the python tf buffer + // the message is serialized to python to be able to transfer it to the + // python tf buffer auto serializer = rclcpp::Serialization(); rclcpp::SerializedMessage serialized_transform; @@ -66,10 +65,8 @@ class TransformListener { // this is the actual serialization serializer.serialize_message(&transform, &serialized_transform); auto rcl_serialized_transform = serialized_transform.get_rcl_serialized_message(); - py::bytes py_serialized_transform = { - reinterpret_cast(rcl_serialized_transform.buffer), - rcl_serialized_transform.buffer_length - }; + py::bytes py_serialized_transform = {reinterpret_cast(rcl_serialized_transform.buffer), + rcl_serialized_transform.buffer_length}; // we have to acquire the GIL to be able to call python functions { // we need PyGILState_Ensure and py::gil_scoped_acquire because of a bug @@ -83,12 +80,13 @@ class TransformListener { // destructor ~TransformListener() { - // the executor finishes when rclcpp is shutdown, so the thread can be joined + // the executor finishes when rclcpp is shutdown, so the thread can be + // joined rclcpp::shutdown(); thread_->join(); } -private: + private: std::shared_ptr node_; std::shared_ptr thread_; std::shared_ptr executor_; diff --git a/bitbots_utils/bitbots_utils/game_settings.py b/bitbots_utils/bitbots_utils/game_settings.py index ce625c49..91761eff 100755 --- a/bitbots_utils/bitbots_utils/game_settings.py +++ b/bitbots_utils/bitbots_utils/game_settings.py @@ -31,13 +31,13 @@ def provide_config(path): """ if os.path.exists(path): try: - with open(path, "r") as f: + with open(path) as f: config = yaml.load(f, Loader=yaml.UnsafeLoader) except yaml.YAMLError as exc: print("Error in configuration file:", exc) else: config = {} - print("The config yaml with path {}, does not exist.".format(path)) + print(f"The config yaml with path {path}, does not exist.") return config @@ -57,7 +57,7 @@ def ask_for_config_option( :param explanation: describes options :return: new chosen value for this config option, can be the old one """ - print("=============== {} ===============".format(name)) + print(f"=============== {name} ===============") if valid_options: print(f"Options: {valid_options}") print(f"Explanations: {explanation}") diff --git a/bitbots_utils/bitbots_utils/transforms.py b/bitbots_utils/bitbots_utils/transforms.py index 24a42c75..bebb0015 100644 --- a/bitbots_utils/bitbots_utils/transforms.py +++ b/bitbots_utils/bitbots_utils/transforms.py @@ -1,20 +1,18 @@ import math -import numpy as np +import numpy as np from geometry_msgs.msg import Quaternion from ros2_numpy import msgify -from transforms3d.euler import quat2euler, euler2quat -from transforms3d.quaternions import quat2mat, mat2quat -from transforms3d.quaternions import rotate_vector, qinverse, quat2mat, mat2quat - +from transforms3d.euler import euler2quat, quat2euler +from transforms3d.quaternions import mat2quat, qinverse, quat2mat, rotate_vector def wxyz2xyzw(quat_wxyz: np.ndarray) -> np.ndarray: - return quat_wxyz[[1,2,3,0]] + return quat_wxyz[[1, 2, 3, 0]] def xyzw2wxyz(quat_xyzw: np.ndarray) -> np.ndarray: - return quat_xyzw[[3,0,1,2]] + return quat_xyzw[[3, 0, 1, 2]] def quat2sixd(quat_wxyz: np.ndarray) -> np.ndarray: @@ -39,18 +37,19 @@ def sixd2quat(sixd): return mat2quat(mat) -def quat2fused(q, order='wxyz'): +def quat2fused(q, order="wxyz"): # Check quaternion order - if order == 'xyzw': + if order == "xyzw": q_xyzw = q - elif order == 'wxyz': + elif order == "wxyz": q_xyzw = wxyz2xyzw(q) else: - raise ValueError('Unknown quaternion order: {}'.format(order)) + raise ValueError(f"Unknown quaternion order: {order}") # Fused yaw of Quaternion - fused_yaw = 2.0 * math.atan2(q_xyzw[2], - q_xyzw[3]) # Output of atan2 is [-tau/2,tau/2], so this expression is in [-tau,tau] + fused_yaw = 2.0 * math.atan2( + q_xyzw[2], q_xyzw[3] + ) # Output of atan2 is [-tau/2,tau/2], so this expression is in [-tau,tau] if fused_yaw > math.tau / 2: fused_yaw -= math.tau # fused_yaw is now in[-2* pi, pi] if fused_yaw <= -math.tau / 2: @@ -71,15 +70,15 @@ def quat2fused(q, order='wxyz'): fused_roll = math.asin(sphi) # compute hemi parameter - hemi = (0.5 - (q_xyzw[0] * q_xyzw[0] + q_xyzw[1] * q_xyzw[1]) >= 0.0) + hemi = 0.5 - (q_xyzw[0] * q_xyzw[0] + q_xyzw[1] * q_xyzw[1]) >= 0.0 return fused_roll, fused_pitch, fused_yaw, hemi # Conversion: Fused angles (3D/4D) --> Quaternion (wxyz) -def fused2quat(fusedRoll, fusedPitch, fusedYaw, hemi): +def fused2quat(fused_roll, fused_pitch, fused_yaw, hemi): # Precalculate the sine values - sth = math.sin(fusedPitch) - sphi = math.sin(fusedRoll) + sth = math.sin(fused_pitch) + sphi = math.sin(fused_roll) # Calculate the sine sum criterion crit = sth * sth + sphi * sphi @@ -98,7 +97,7 @@ def fused2quat(fusedRoll, fusedPitch, fusedYaw, hemi): # Evaluate the required intermediate angles halpha = 0.5 * alpha - hpsi = 0.5 * fusedYaw + hpsi = 0.5 * fused_yaw hgampsi = gamma + hpsi # Precalculate trigonometric terms involved in the quaternion expression @@ -116,13 +115,13 @@ def fused2quat(fusedRoll, fusedPitch, fusedYaw, hemi): def compute_imu_orientation_from_world(robot_quat_in_world): # imu orientation has roll and pitch relative to gravity vector. yaw in world frame # get global yaw - yrp_world_frame = quat2euler(robot_quat_in_world, axes='szxy') + yrp_world_frame = quat2euler(robot_quat_in_world, axes="szxy") # remove global yaw rotation from roll and pitch - yaw_quat = euler2quat(yrp_world_frame[0], 0, 0, axes='szxy') + yaw_quat = euler2quat(yrp_world_frame[0], 0, 0, axes="szxy") rp = rotate_vector((yrp_world_frame[1], yrp_world_frame[2], 0), qinverse(yaw_quat)) # save in correct order return [rp[0], rp[1], 0], yaw_quat def quat_from_yaw(yaw: float) -> Quaternion: - return msgify(Quaternion, wxyz2xyzw(euler2quat(yaw, 0, 0, axes='szxy'))) + return msgify(Quaternion, wxyz2xyzw(euler2quat(yaw, 0, 0, axes="szxy"))) diff --git a/bitbots_utils/bitbots_utils/utils.py b/bitbots_utils/bitbots_utils/utils.py index 71562bc3..35d30627 100644 --- a/bitbots_utils/bitbots_utils/utils.py +++ b/bitbots_utils/bitbots_utils/utils.py @@ -1,36 +1,39 @@ -from typing import Any, Dict, List - import os +from typing import Any, Dict, List -import yaml import rclpy -from rclpy.node import Node +import yaml from ament_index_python import get_package_share_directory from rcl_interfaces.msg import Parameter as ParameterMsg -from rcl_interfaces.msg import ParameterValue as ParameterValueMsg from rcl_interfaces.msg import ParameterType as ParameterTypeMsg +from rcl_interfaces.msg import ParameterValue as ParameterValueMsg from rcl_interfaces.srv import GetParameters, SetParameters -from rclpy.parameter import parameter_value_to_python, Parameter, PARAMETER_SEPARATOR_STRING +from rclpy.node import Node +from rclpy.parameter import PARAMETER_SEPARATOR_STRING, Parameter, parameter_value_to_python def read_urdf(robot_name): - urdf = os.popen(f"xacro {get_package_share_directory(f'{robot_name}_description')}" - f"/urdf/robot.urdf use_fake_walk:=false sim_ns:=false").read() + urdf = os.popen( + f"xacro {get_package_share_directory(f'{robot_name}_description')}" + f"/urdf/robot.urdf use_fake_walk:=false sim_ns:=false" + ).read() return urdf def load_moveit_parameter(robot_name): moveit_parameters = get_parameters_from_plain_yaml( f"{get_package_share_directory(f'{robot_name}_moveit_config')}/config/kinematics.yaml", - "robot_description_kinematics.") + "robot_description_kinematics.", + ) robot_description = ParameterMsg() robot_description.name = "robot_description" - robot_description.value = ParameterValueMsg(string_value=read_urdf(robot_name), - type=ParameterTypeMsg.PARAMETER_STRING) + robot_description.value = ParameterValueMsg( + string_value=read_urdf(robot_name), type=ParameterTypeMsg.PARAMETER_STRING + ) moveit_parameters.append(robot_description) robot_description_semantic = ParameterMsg() robot_description_semantic.name = "robot_description_semantic" - with open(f"{get_package_share_directory(f'{robot_name}_moveit_config')}/config/{robot_name}.srdf", "r") as file: + with open(f"{get_package_share_directory(f'{robot_name}_moveit_config')}/config/{robot_name}.srdf") as file: value = file.read() robot_description_semantic.value = ParameterValueMsg(string_value=value, type=ParameterTypeMsg.PARAMETER_STRING) moveit_parameters.append(robot_description_semantic) @@ -39,29 +42,32 @@ def load_moveit_parameter(robot_name): def get_parameters_from_ros_yaml(node_name, parameter_file, use_wildcard): # Remove leading slash and namespaces - with open(parameter_file, 'r') as f: + with open(parameter_file) as f: param_file = yaml.safe_load(f) param_keys = [] - if use_wildcard and '/**' in param_file: - param_keys.append('/**') + if use_wildcard and "/**" in param_file: + param_keys.append("/**") if node_name in param_file: param_keys.append(node_name) if param_keys == []: - raise RuntimeError('Param file does not contain parameters for {}, ' - ' only for nodes: {}'.format(node_name, param_file.keys())) + raise RuntimeError( + f"Param file does not contain parameters for {node_name}, " f" only for nodes: {param_file.keys()}" + ) param_dict = {} for k in param_keys: value = param_file[k] - if type(value) != dict or 'ros__parameters' not in value: - raise RuntimeError('Invalid structure of parameter file for node {}' - 'expected same format as provided by ros2 param dump'.format(k)) - param_dict.update(value['ros__parameters']) - return parse_parameter_dict(namespace='', parameter_dict=param_dict) + if isinstance(value, dict) or "ros__parameters" not in value: + raise RuntimeError( + f"Invalid structure of parameter file for node {k}" + "expected same format as provided by ros2 param dump" + ) + param_dict.update(value["ros__parameters"]) + return parse_parameter_dict(namespace="", parameter_dict=param_dict) -def get_parameters_from_plain_yaml(parameter_file, namespace=''): - with open(parameter_file, 'r') as f: +def get_parameters_from_plain_yaml(parameter_file, namespace=""): + with open(parameter_file) as f: param_dict = yaml.safe_load(f) return parse_parameter_dict(namespace=namespace, parameter_dict=param_dict) @@ -81,9 +87,9 @@ def get_parameter_dict(node: Node, prefix: str) -> Dict: config = dict() for param in parameter_config.values(): # Split separated keys into nested dicts - param_nests = param.name[len(prefix):].split(PARAMETER_SEPARATOR_STRING) + param_nests = param.name[len(prefix) :].split(PARAMETER_SEPARATOR_STRING) # Remove empty first element from split (because of possible leading separator) - if param_nests[0] == '': + if param_nests[0] == "": param_nests.pop(0) # Traverse nested dicts and create them if they don't exist param_dict = config @@ -101,18 +107,17 @@ def get_parameter_dict(node: Node, prefix: str) -> Dict: return config -def get_parameters_from_other_node(own_node: Node, - other_node_name: str, - parameter_names: List[str], - service_timeout_sec: float = 20.0) -> Dict: +def get_parameters_from_other_node( + own_node: Node, other_node_name: str, parameter_names: List[str], service_timeout_sec: float = 20.0 +) -> Dict: """ Used to receive parameters from other running nodes. Returns a dict with requested parameter name as dict key and parameter value as dict value. """ - client = own_node.create_client(GetParameters, f'{other_node_name}/get_parameters') + client = own_node.create_client(GetParameters, f"{other_node_name}/get_parameters") ready = client.wait_for_service(timeout_sec=service_timeout_sec) if not ready: - raise RuntimeError(f'Wait for {other_node_name} parameter service timed out') + raise RuntimeError(f"Wait for {other_node_name} parameter service timed out") request = GetParameters.Request() request.names = parameter_names future = client.call_async(request) @@ -125,19 +130,21 @@ def get_parameters_from_other_node(own_node: Node, return results -def set_parameters_of_other_node(own_node: Node, - other_node_name: str, - parameter_names: List[str], - parameter_values: List[Any], - service_timeout_sec: float = 20.0) -> List[bool]: +def set_parameters_of_other_node( + own_node: Node, + other_node_name: str, + parameter_names: List[str], + parameter_values: List[Any], + service_timeout_sec: float = 20.0, +) -> List[bool]: """ Used to set parameters of another running node. Returns a list of booleans indicating success or failure. """ - client = own_node.create_client(SetParameters, f'{other_node_name}/set_parameters') + client = own_node.create_client(SetParameters, f"{other_node_name}/set_parameters") ready = client.wait_for_service(timeout_sec=service_timeout_sec) if not ready: - raise RuntimeError(f'Wait for {other_node_name} parameter service timed out') + raise RuntimeError(f"Wait for {other_node_name} parameter service timed out") request = SetParameters.Request() for name, value in zip(parameter_names, parameter_values): @@ -156,10 +163,10 @@ def parse_parameter_dict(*, namespace, parameter_dict): for param_name, param_value in parameter_dict.items(): full_param_name = namespace + param_name # Unroll nested parameters - if type(param_value) == dict: + if isinstance(param_value, dict): parameters += parse_parameter_dict( - namespace=full_param_name + PARAMETER_SEPARATOR_STRING, - parameter_dict=param_value) + namespace=full_param_name + PARAMETER_SEPARATOR_STRING, parameter_dict=param_value + ) else: parameter = Parameter(name=full_param_name, value=param_value) parameters.append(parameter.to_parameter_msg()) diff --git a/bitbots_utils/docs/conf.py b/bitbots_utils/docs/conf.py index 586c7734..0bf6043d 100644 --- a/bitbots_utils/docs/conf.py +++ b/bitbots_utils/docs/conf.py @@ -1,4 +1,3 @@ -# -*- coding: utf-8 -*- # # Full list of options at http://www.sphinx-doc.org/en/master/config @@ -10,28 +9,30 @@ # import os import sys -import catkin_pkg.package +import catkin_pkg.package from exhale import utils package_dir = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) catkin_package = catkin_pkg.package.parse_package( - os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME)) + os.path.join(package_dir, catkin_pkg.package.PACKAGE_MANIFEST_FILENAME) +) sys.path.insert(0, os.path.abspath(os.path.join(package_dir, "src"))) # -- Helper functions -------------------------------------------------------- + def count_files(): """:returns tuple of (num_py, num_cpp)""" num_py = 0 num_cpp = 0 - for root, dirs, files in os.walk(os.path.join(package_dir, "src")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "src")): for f in files: if f.endswith(".py"): num_py += 1 - for root, dirs, files in os.walk(os.path.join(package_dir, "include")): + for _root, _dirs, files in os.walk(os.path.join(package_dir, "include")): for f in files: if f.endswith(".h") or f.endswith(".hpp"): num_cpp += 1 @@ -42,7 +43,7 @@ def count_files(): # -- Project information ----------------------------------------------------- project = catkin_package.name -copyright = '2019, Bit-Bots' +copyright = "2019, Bit-Bots" author = ", ".join([a.name for a in catkin_package.authors]) # The short X.Y version @@ -60,27 +61,27 @@ def count_files(): # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom # ones. extensions = [ - 'sphinx.ext.autodoc', - 'sphinx.ext.doctest', - 'sphinx.ext.intersphinx', - 'sphinx.ext.todo', - 'sphinx.ext.coverage', - 'sphinx.ext.imgmath', - 'sphinx.ext.viewcode', - 'sphinx_rtd_theme', + "sphinx.ext.autodoc", + "sphinx.ext.doctest", + "sphinx.ext.intersphinx", + "sphinx.ext.todo", + "sphinx.ext.coverage", + "sphinx.ext.imgmath", + "sphinx.ext.viewcode", + "sphinx_rtd_theme", ] # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # The suffix(es) of source filenames. # You can specify multiple suffix as a list of string: # # source_suffix = ['.rst', '.md'] -source_suffix = '.rst' +source_suffix = ".rst" # The master toctree document. -master_doc = 'index' +master_doc = "index" # The language for content autogenerated by Sphinx. Refer to documentation # for a list of supported languages. @@ -92,7 +93,7 @@ def count_files(): # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # The name of the Pygments (syntax highlighting) style to use. pygments_style = None @@ -108,24 +109,17 @@ def count_files(): if num_files_cpp > 0: extensions += [ - 'breathe', - 'exhale', + "breathe", + "exhale", ] - breathe_projects = { - project: os.path.join("_build", "doxyoutput", "xml") - } + breathe_projects = {project: os.path.join("_build", "doxyoutput", "xml")} breathe_default_project = project def specifications_for_kind(kind): # Show all members for classes and structs if kind == "class" or kind == "struct": - return [ - ":members:", - ":protected-members:", - ":private-members:", - ":undoc-members:" - ] + return [":members:", ":protected-members:", ":private-members:", ":undoc-members:"] # An empty list signals to Exhale to use the defaults else: return [] @@ -136,13 +130,11 @@ def specifications_for_kind(kind): "rootFileName": "library_root.rst", "rootFileTitle": "C++ Library API", "doxygenStripFromPath": "..", - "customSpecificationsMapping": utils.makeCustomSpecificationsMapping( - specifications_for_kind - ), + "customSpecificationsMapping": utils.makeCustomSpecificationsMapping(specifications_for_kind), # Suggested optional arguments "createTreeView": True, "exhaleExecutesDoxygen": True, - "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")) + "exhaleDoxygenStdin": "INPUT = {}".format(os.path.join(package_dir, "include")), } # -- Options for HTML output ------------------------------------------------- @@ -150,7 +142,7 @@ def specifications_for_kind(kind): # The theme to use for HTML and HTML Help pages. See the documentation for # a list of builtin themes. # -html_theme = 'sphinx_rtd_theme' +html_theme = "sphinx_rtd_theme" # Theme options are theme-specific and customize the look and feel of a theme # further. For a list of options available for each theme, see the @@ -161,7 +153,7 @@ def specifications_for_kind(kind): # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, # so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ['_static'] +html_static_path = ["_static"] # Custom sidebar templates, must be a dictionary that maps document names # to template names. @@ -173,14 +165,14 @@ def specifications_for_kind(kind): # # html_sidebars = {} -html_logo = os.path.join('_static', 'logo.png') -html_favicon = os.path.join('_static', 'logo.png') +html_logo = os.path.join("_static", "logo.png") +html_favicon = os.path.join("_static", "logo.png") # -- Options for intersphinx extension --------------------------------------- # Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {'https://docs.python.org/': None} +intersphinx_mapping = {"https://docs.python.org/": None} # -- Options for todo extension ---------------------------------------------- @@ -188,6 +180,8 @@ def specifications_for_kind(kind): todo_include_todos = True # -- RST Standard variables --------------------------------------------------- -rst_prolog = ".. |project| replace:: {}\n".format(project) +rst_prolog = f".. |project| replace:: {project}\n" rst_prolog += ".. |description| replace:: {}\n".format(catkin_package.description.replace("\n\n", "\n")) -rst_prolog += ".. |modindex| replace:: {}\n".format(":ref:`modindex`" if num_files_py > 0 else "Python module index is not available") +rst_prolog += ".. |modindex| replace:: {}\n".format( + ":ref:`modindex`" if num_files_py > 0 else "Python module index is not available" +) diff --git a/bitbots_utils/include/bitbots_utils/utils.hpp b/bitbots_utils/include/bitbots_utils/utils.hpp index 5503c87d..91f9d3d9 100644 --- a/bitbots_utils/include/bitbots_utils/utils.hpp +++ b/bitbots_utils/include/bitbots_utils/utils.hpp @@ -1,17 +1,16 @@ #ifndef BITBOTS_UTILS__UTILS_H_ #define BITBOTS_UTILS__UTILS_H_ -#include +#include #include +#include #include -#include -#include +#include using namespace std::chrono_literals; -namespace bitbots_utils -{ +namespace bitbots_utils { /** * @brief Waits for the transforms to be available @@ -21,20 +20,17 @@ namespace bitbots_utils * @param frames The tf frames to wait for * @param root_frame The root frame to transform from * @param check_interval Interval in which to check for the frames - * @param warn_duration Duration after which to warn if the frames are not available - * @param warn_interval Interval in which to keep warning if the frames are not available + * @param warn_duration Duration after which to warn if the frames are not + * available + * @param warn_interval Interval in which to keep warning if the frames are not + * available * @param verbose Can be used to disable the warning messages */ -void wait_for_tf( - const rclcpp::Logger &logger, - std::shared_ptr clock, - std::shared_ptr tf_buffer, - const std::vector &frames, - const std::string &root_frame, - const rclcpp::Duration &check_interval = rclcpp::Duration(0.1s), - const rclcpp::Duration &warn_duration = rclcpp::Duration(5.0s), - const rclcpp::Duration &warn_interval = rclcpp::Duration(1.0s), - bool verbose = true); +void wait_for_tf(const rclcpp::Logger &logger, std::shared_ptr clock, + std::shared_ptr tf_buffer, const std::vector &frames, + const std::string &root_frame, const rclcpp::Duration &check_interval = rclcpp::Duration(0.1s), + const rclcpp::Duration &warn_duration = rclcpp::Duration(5.0s), + const rclcpp::Duration &warn_interval = rclcpp::Duration(1.0s), bool verbose = true); } // namespace bitbots_utils diff --git a/bitbots_utils/scripts/check_robot.py b/bitbots_utils/scripts/check_robot.py index 7770f87c..b94ab61d 100755 --- a/bitbots_utils/scripts/check_robot.py +++ b/bitbots_utils/scripts/check_robot.py @@ -1,48 +1,52 @@ #!/usr/bin/python3 import os +import sys +import actionlib +import dynamic_reconfigure.client import rclpy -from rclpy.node import Node -import rosnode import roslaunch +import rosnode import rospkg import rostopic -from bitbots_msgs.msg import FootPressure +from actionlib.msg import GoalStatus +from bitbots_msgs.msg import FootPressure, KickAction, KickGoal from diagnostic_msgs.msg import DiagnosticStatus -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Quaternion, Twist +from sensor_msgs.msg import Imu, JointState from std_srvs.srv import Empty -import actionlib -from bitbots_msgs.msg import KickGoal, KickAction, KickFeedback -from geometry_msgs.msg import Vector3, Quaternion from tf.transformations import quaternion_from_euler -import dynamic_reconfigure.client -from sensor_msgs.msg import JointState, Imu -class bcolors: - HEADER = '\033[95m' - OKBLUE = '\033[94m' - OKGREEN = '\033[92m' - WARNING = '\033[93m' - FAIL = '\033[91m' - ENDC = '\033[0m' - BOLD = '\033[1m' - UNDERLINE = '\033[4m' + +class BColors: + HEADER = "\033[95m" + OKBLUE = "\033[94m" + OKGREEN = "\033[92m" + WARNING = "\033[93m" + FAIL = "\033[91m" + ENDC = "\033[0m" + BOLD = "\033[1m" + UNDERLINE = "\033[4m" def print_warn(str): - print(bcolors.WARNING + str + bcolors.ENDC) + print(BColors.WARNING + str + BColors.ENDC) + def print_info(str): - print(bcolors.OKGREEN + str + bcolors.ENDC) + print(BColors.OKGREEN + str + BColors.ENDC) + def input_info(str): - return input(bcolors.OKBLUE + str + bcolors.ENDC) + return input(BColors.OKBLUE + str + BColors.ENDC) + diag_status = None had_diag_error = False had_diag_stale = False had_diag_warn = False + def diagnostic_cb(msg: DiagnosticStatus): global diag_status, had_diag_warn, had_diag_error, had_diag_stale diag_status = msg.level @@ -71,10 +75,19 @@ def pressure_cb(msg, is_left=True): def is_motion_started(): node_names = rosnode.get_node_names("/") started = True - nodes_in_motion = {"/ros_control", "/hcm", "/walking", "/animation", "/dynamic_kick", "/motion_odometry", "/odometry_fuser", "/DynupNode"} + nodes_in_motion = { + "/ros_control", + "/hcm", + "/walking", + "/animation", + "/dynamic_kick", + "/motion_odometry", + "/odometry_fuser", + "/DynupNode", + } for node in nodes_in_motion: - if not node in node_names: - print_info(F"{node} not running") + if node not in node_names: + print_info(f"{node} not running") started = False return started @@ -82,29 +95,35 @@ def is_motion_started(): def check_pressure(msg, min, max, foot_name): okay = True if msg.left_front < min or msg.left_front > max: - print_warn(F" {foot_name} left_front out of limits. Min {min} Max {max} Value {round(msg.left_front, 2)}\n") + print_warn(f" {foot_name} left_front out of limits. Min {min} Max {max} Value {round(msg.left_front, 2)}\n") okay = False if msg.left_back < min or msg.left_back > max: - print_warn(F" {foot_name} left_back out of limits. Min {min} Max {max} Value {round(msg.left_back, 2)}\n") + print_warn(f" {foot_name} left_back out of limits. Min {min} Max {max} Value {round(msg.left_back, 2)}\n") okay = False if msg.right_back < min or msg.right_back > max: - print_warn(F" {foot_name} right_back out of limits. Min {min} Max {max} Value {round(msg.right_back, 2)}\n") + print_warn(f" {foot_name} right_back out of limits. Min {min} Max {max} Value {round(msg.right_back, 2)}\n") okay = False if msg.right_front < min or msg.right_front > max: - print_warn(F" {foot_name} right_front out of limits. Min {min} Max {max} Value {round(msg.right_front, 2)}\n") + print_warn(f" {foot_name} right_front out of limits. Min {min} Max {max} Value {round(msg.right_front, 2)}\n") okay = False return okay + got_pwm = False -def pwm_callback(msg:JointState): + + +def pwm_callback(msg: JointState): global got_pwm - got_pwm= True + got_pwm = True for effort in msg.effort: if effort == 100: print_warn("Servo reported max PWM value. It is working at its limit!\n") + ACCEL_LIMIT = 35 ANGULAR_VEL_LIMIT = 10 + + def imu_callback(msg: Imu): for accel in [msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]: if abs(accel) > ACCEL_LIMIT: @@ -113,39 +132,42 @@ def imu_callback(msg: Imu): if abs(angular_vel) > ANGULAR_VEL_LIMIT: print_warn("IMU over angular vel limit! Orientation estimation will suffer.\n") -if __name__ == '__main__': + +if __name__ == "__main__": print_info("### This script will check the robot hardware and motions. Please follow the instructions\n") rclpy.init(args=None) # start subscribers - imu_sub = rospy.Subscriber("imu/data", Imu, imu_callback, tcp_nodelay=True) - pwm_sub = rospy.Subscriber("servo_PWM", JointState, pwm_callback, tcp_nodelay=True) - + imu_sub = rclpy.Subscriber("imu/data", Imu, imu_callback, tcp_nodelay=True) + pwm_sub = rclpy.Subscriber("servo_PWM", JointState, pwm_callback, tcp_nodelay=True) # start necessary software - print_info("First the motion software will be started. Please hold the robot, turn on servo power and press enter.\n") + print_info( + "First the motion software will be started. Please hold the robot, turn on servo power and press enter.\n" + ) input_info("Press Enter to continue...") uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) rospack = rospkg.RosPack() - launch = roslaunch.parent.ROSLaunchParent(uuid, [ - rospack.get_path('bitbots_bringup') + "/launch/motion_standalone.launch"]) + launch = roslaunch.parent.ROSLaunchParent( + uuid, [rospack.get_path("bitbots_bringup") + "/launch/motion_standalone.launch"] + ) launch.start() - rospy.sleep(5) + rclpy.sleep(5) while True: if is_motion_started(): - rospy.sleep(5) + rclpy.sleep(5) break else: print_info("Waiting for software to be started \n") - rospy.sleep(1) + rclpy.sleep(1) print_info("\n\n") # check diagnostic status print_info("Will check diagnostic status of robot.\n") - diag_sub = rospy.Subscriber("diagnostics_toplevel_state", DiagnosticStatus, diagnostic_cb, tcp_nodelay=True) - rospy.sleep(3) + diag_sub = rclpy.Subscriber("diagnostics_toplevel_state", DiagnosticStatus, diagnostic_cb, tcp_nodelay=True) + rclpy.sleep(3) if not (had_diag_stale or had_diag_warn or had_diag_error): print_info(" Diagnostic status okay.") @@ -165,42 +187,51 @@ def imu_callback(msg: Imu): # check publishing frequency of imu, servos, pwm, goals, pressure, robot_state # the topics which will be checked with minimal publishing rate - topics_to_check = {"imu/data": 900, "joint_states": 900, "robot_state": 900, "foot_pressure_left/raw": 900, - "foot_pressure_left/filtered": 900, "foot_pressure_right/raw": 900, - "foot_pressure_right/filtered": 900, "core/vdxl": 9, "diagnostics_toplevel_state": 9} + topics_to_check = { + "imu/data": 900, + "joint_states": 900, + "robot_state": 900, + "foot_pressure_left/raw": 900, + "foot_pressure_left/filtered": 900, + "foot_pressure_right/raw": 900, + "foot_pressure_right/filtered": 900, + "core/vdxl": 9, + "diagnostics_toplevel_state": 9, + } rts = [] for topic in topics_to_check.keys(): msg_class, real_topic, _ = rostopic.get_topic_class(topic) if real_topic is None or msg_class is None: - print_warn(F"Problem with topic {topic}") + print_warn(f"Problem with topic {topic}") else: rt = rostopic.ROSTopicHz(-1) - rt_sub = rospy.Subscriber(topic, msg_class, rt.callback_hz, callback_args=topic, tcp_nodelay=True) + rt_sub = rclpy.Subscriber(topic, msg_class, rt.callback_hz, callback_args=topic, tcp_nodelay=True) rts.append((rt, rt_sub)) print_info("Please wait a few seconds for publishing rates of topics to be evaluated\n") - rospy.sleep(5) + rclpy.sleep(5) print_info("Topics have been evaluated:\n") i = 0 for topic in topics_to_check.keys(): rate = rts[i][0].get_hz(topic)[0] if rate is None or rate < topics_to_check[topic]: - print_warn(F" Low rate on Topic {topic}: {round(rate, 2)} \n") + print_warn(f" Low rate on Topic {topic}: {round(rate, 2)} \n") else: - print_info(F" Okay rate Topic {topic}: {round(rate, 2)} \n") + print_info(f" Okay rate Topic {topic}: {round(rate, 2)} \n") i += 1 # check pressure values when robot in air print_info("\n\n") print_info("We will check the foot pressure sensors next\n") input_info("Please hold the robot in the air so that the feet don't touch the ground and press enter.") - left_pressure_sub = rospy.Subscriber("foot_pressure_left/filtered", FootPressure, pressure_cb, callback_args=True, - tcp_nodelay=True) - right_pressure_sub = rospy.Subscriber("foot_pressure_right/filtered", FootPressure, pressure_cb, - callback_args=False, - tcp_nodelay=True) - rospy.sleep(0.5) + left_pressure_sub = rclpy.Subscriber( + "foot_pressure_left/filtered", FootPressure, pressure_cb, callback_args=True, tcp_nodelay=True + ) + right_pressure_sub = rclpy.Subscriber( + "foot_pressure_right/filtered", FootPressure, pressure_cb, callback_args=False, tcp_nodelay=True + ) + rclpy.sleep(0.5) while (not left_pressure) and (not right_pressure) and (rclpy.ok()): - rospy.loginfo_throttle(1, "Waiting to receive pressure msgs\n") + rclpy.loginfo_throttle(1, "Waiting to receive pressure msgs\n") print_info("Pressure messages received\n") both_okay = True both_okay = both_okay and check_pressure(left_pressure, -1, 1, "left") @@ -208,12 +239,12 @@ def imu_callback(msg: Imu): if not both_okay: print_warn("Pressure not correctly zero. Will try to call zero service.\n") # call zero service - zero_l = rospy.ServiceProxy("foot_pressure_left/set_foot_zero", Empty) - zero_r = rospy.ServiceProxy("foot_pressure_right/set_foot_zero", Empty) + zero_l = rclpy.ServiceProxy("foot_pressure_left/set_foot_zero", Empty) + zero_r = rclpy.ServiceProxy("foot_pressure_right/set_foot_zero", Empty) zero_l() zero_r() # wait and check again - rospy.sleep(1) + rclpy.sleep(1) both_okay = True both_okay = both_okay and check_pressure(left_pressure, -1, 1, "left") both_okay = both_okay and check_pressure(right_pressure, -1, 1, "right") @@ -230,94 +261,106 @@ def imu_callback(msg: Imu): if both_okay: print_info("Pressure seems to be okay\n") else: - print_warn("Problem with pressure. " - "Please recalibrate the sensors using rosrun bitbots_ros_control pressure_calibaration\n") + print_warn( + "Problem with pressure. " + "Please recalibrate the sensors using rosrun bitbots_ros_control pressure_calibaration\n" + ) # check servo PWM status - print_info("We will check the servo PWM status now. This gives information if any servo is going on maximum torque.\n") + print_info( + "We will check the servo PWM status now. This gives information if any servo is going on maximum torque.\n" + ) print_info("Will now try to activate PWM reading and wait till they are recieved.\n") dyn_reconf_client = dynamic_reconfigure.client.Client("/ros_control/servos", timeout=10.0) - dyn_reconf_client.update_configuration({'read_pwm': True}) + dyn_reconf_client.update_configuration({"read_pwm": True}) while not got_pwm: - rospy.sleep(0.1) + rclpy.sleep(0.1) print_info("Got PWM values, will continue.\n") # check fall front - input_info("\nNext we will check front falling detection.\n " - "Please let the robot fall on its belly, but hold it to prevent damage. " - "The robot should perform a safety motion. Afterwards it should stand up by itself. " - "Press enter when you're done.\n") - - input_info("\nNext we will check back falling detection.\n " - "Please let the robot fall on its back, but hold it to prevent damage. " - "The robot should perform a safety motion. Afterwards it should stand up by itself. " - "Press enter when you're done.\n") + input_info( + "\nNext we will check front falling detection.\n " + "Please let the robot fall on its belly, but hold it to prevent damage. " + "The robot should perform a safety motion. Afterwards it should stand up by itself. " + "Press enter when you're done.\n" + ) + + input_info( + "\nNext we will check back falling detection.\n " + "Please let the robot fall on its back, but hold it to prevent damage. " + "The robot should perform a safety motion. Afterwards it should stand up by itself. " + "Press enter when you're done.\n" + ) # check walk motion - walk_pub = self.create_publisher(Twist, "/cmd_vel", 1) + walk_pub = rclpy.create_publisher(Twist, "/cmd_vel", 1) text = input_info( "\nWe will check walking of the robot. After pressing enter, robot will start walking in different directions. " - "It will stop when it is finished. Please make sure there is space and catch it if it falls. Press y if you want to check walking.") + "It will stop when it is finished. Please make sure there is space and catch it if it falls. Press y if you want to check walking." + ) if text == "y": cmd_msg = Twist() cmd_msg.linear.x = 0.1 walk_pub.publish(cmd_msg) - rospy.sleep(5) + rclpy.sleep(5) cmd_msg.linear.x = -0.1 walk_pub.publish(cmd_msg) - rospy.sleep(5) + rclpy.sleep(5) cmd_msg.linear.x = 0.0 cmd_msg.linear.y = 0.1 walk_pub.publish(cmd_msg) - rospy.sleep(5) + rclpy.sleep(5) cmd_msg.linear.y = -0.1 walk_pub.publish(cmd_msg) - rospy.sleep(5) + rclpy.sleep(5) cmd_msg.linear.y = 0 walk_pub.publish(cmd_msg) - rospy.sleep(1) + rclpy.sleep(1) # check kick motion - text = input_info("\nWe will check the kick motion. Please hold make sure the robot is safe. " - "Press y if you want to perform this test.") - if text == 'y': + text = input_info( + "\nWe will check the kick motion. Please hold make sure the robot is safe. " + "Press y if you want to perform this test." + ) + if text == "y": + def done_cb(state, result): - print_info('Action completed: ', end='') + print_info("Action completed: ", end="") if state == GoalStatus.PENDING: - print_info('Pending') + print_info("Pending") elif state == GoalStatus.ACTIVE: - print_info('Active') + print_info("Active") elif state == GoalStatus.PREEMPTED: - print_info('Preempted') + print_info("Preempted") elif state == GoalStatus.SUCCEEDED: - print_info('Succeeded') + print_info("Succeeded") elif state == GoalStatus.ABORTED: - print_info('Aborted') + print_info("Aborted") elif state == GoalStatus.REJECTED: - print_info('Rejected') + print_info("Rejected") elif state == GoalStatus.PREEMPTING: - print_info('Preempting') + print_info("Preempting") elif state == GoalStatus.RECALLING: - print_info('Recalling') + print_info("Recalling") elif state == GoalStatus.RECALLED: - print_info('Recalled') + print_info("Recalled") elif state == GoalStatus.LOST: - print_info('Lost') + print_info("Lost") else: - print_info('Unknown state', state) + print_info("Unknown state", state) print_info(str(result)) def active_cb(): print_info("Server accepted action") def feedback_cb(feedback): - if len(sys.argv) > 1 and sys.argv[1] == '--feedback': - print_info('Feedback') + if len(sys.argv) > 1 and sys.argv[1] == "--feedback": + print_info("Feedback") print_info(feedback) print_info() goal = KickGoal() - goal.header.stamp = self.get_clock().now() + goal.header.stamp = rclpy.get_clock().now() goal.header.frame_id = "base_footprint" goal.ball_position.x = 0.2 goal.ball_position.y = -0.09 @@ -325,7 +368,7 @@ def feedback_cb(feedback): goal.kick_direction = Quaternion(*quaternion_from_euler(0, 0, 0)) goal.kick_speed = 1 - client = actionlib.SimpleActionClient('dynamic_kick', KickAction) + client = actionlib.SimpleActionClient("dynamic_kick", KickAction) client.done_cb = done_cb client.feedback_cb = feedback_cb client.active_cb = active_cb @@ -343,7 +386,6 @@ def feedback_cb(feedback): if had_diag_stale: print_warn(" There were stales.") - print_info("Will now call roswtf to investigate if there are any issues") os.system("roswtf") diff --git a/bitbots_utils/scripts/dummy_imu.py b/bitbots_utils/scripts/dummy_imu.py index 0151082a..402cf7f6 100755 --- a/bitbots_utils/scripts/dummy_imu.py +++ b/bitbots_utils/scripts/dummy_imu.py @@ -1,17 +1,16 @@ #!/usr/bin/env python3 import rclpy -from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node from sensor_msgs.msg import Imu class DummyImu(Node): - def __init__(self): - super().__init__('DummyImu') - self.pub = self.create_publisher(Imu, '/imu/data', 1) + super().__init__("DummyImu") + self.pub = self.create_publisher(Imu, "/imu/data", 1) self.msg = Imu() - self.msg.header.frame_id = 'imu_frame' + self.msg.header.frame_id = "imu_frame" self.msg.orientation.w = 1.0 def loop(self): @@ -19,7 +18,7 @@ def loop(self): self.pub.publish(self.msg) -if __name__ == '__main__': +if __name__ == "__main__": rclpy.init(args=None) node = DummyImu() diff --git a/bitbots_utils/scripts/game_settings.py b/bitbots_utils/scripts/game_settings.py index 677f1ec1..47d97e60 100755 --- a/bitbots_utils/scripts/game_settings.py +++ b/bitbots_utils/scripts/game_settings.py @@ -1,12 +1,11 @@ #!/usr/bin/env python3 -import sys import os +import sys sys.path.append(os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "src")) -from bitbots_utils import game_settings - +from bitbots_utils import game_settings # noqa if __name__ == "__main__": game_settings.main() diff --git a/bitbots_utils/scripts/motor_goals_viz_helper.py b/bitbots_utils/scripts/motor_goals_viz_helper.py index 53d085c9..839e74ed 100755 --- a/bitbots_utils/scripts/motor_goals_viz_helper.py +++ b/bitbots_utils/scripts/motor_goals_viz_helper.py @@ -4,7 +4,6 @@ import sys import rclpy - from bitbots_msgs.msg import Animation, JointCommand from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node @@ -14,7 +13,7 @@ class MotorVizHelper(Node): def __init__(self): - super().__init__('MotorGoalsVizHelper') + super().__init__("MotorGoalsVizHelper") # get rid of additional ROS args when used in launch file parser = argparse.ArgumentParser() @@ -33,29 +32,103 @@ def __init__(self): if self.args.robot_type == "wolfgang": # List of all joint names. Do not change the order as it is important for Gazebo - self.joint_names = ['HeadPan', 'HeadTilt', 'LShoulderPitch', 'LShoulderRoll', 'LElbow', 'RShoulderPitch', - 'RShoulderRoll', 'RElbow', 'LHipYaw', 'LHipRoll', 'LHipPitch', 'LKnee', 'LAnklePitch', - 'LAnkleRoll', 'RHipYaw', 'RHipRoll', 'RHipPitch', 'RKnee', 'RAnklePitch', 'RAnkleRoll'] - self.joint_goals = [float(0), float(0), float(0), float(0), float(0), float(0), float(0), float(0), - float(0), float(0), float(0.7), float(-1), float(-0.4), float(0), float(0), float(0), - float(-0.7), float(1), float(0.4), float(0)] + self.joint_names = [ + "HeadPan", + "HeadTilt", + "LShoulderPitch", + "LShoulderRoll", + "LElbow", + "RShoulderPitch", + "RShoulderRoll", + "RElbow", + "LHipYaw", + "LHipRoll", + "LHipPitch", + "LKnee", + "LAnklePitch", + "LAnkleRoll", + "RHipYaw", + "RHipRoll", + "RHipPitch", + "RKnee", + "RAnklePitch", + "RAnkleRoll", + ] + self.joint_goals = [ + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + 0.7, + float(-1), + float(-0.4), + float(0), + float(0), + float(0), + float(-0.7), + float(1), + 0.4, + float(0), + ] elif self.args.robot_type == "itandroids": - self.joint_names = ["rightShoulderPitch[shoulder]", "leftShoulderPitch[shoulder]", - "rightShoulderYaw", "leftShoulderYaw", "rightElbowYaw", "leftElbowYaw", "rightHipYaw", - "leftHipYaw", "rightHipRoll[hip]", "leftHipRoll[hip]", "rightHipPitch", - "leftHipPitch", "rightKneePitch", "leftKneePitch", "rightAnklePitch", "leftAnklePitch", - "rightAnkleRoll", "leftAnkleRoll", "neckYaw", "neckPitch"] - self.joint_goals = [float(0), float(0), float(0), float(0), float(0), float(0), float(0), float(0), - float(0), float(0), float(0), float(0), float(0), float(0), float(0), float(0), - float(0), float(0), float(0), float(0)] + self.joint_names = [ + "rightShoulderPitch[shoulder]", + "leftShoulderPitch[shoulder]", + "rightShoulderYaw", + "leftShoulderYaw", + "rightElbowYaw", + "leftElbowYaw", + "rightHipYaw", + "leftHipYaw", + "rightHipRoll[hip]", + "leftHipRoll[hip]", + "rightHipPitch", + "leftHipPitch", + "rightKneePitch", + "leftKneePitch", + "rightAnklePitch", + "leftAnklePitch", + "rightAnkleRoll", + "leftAnkleRoll", + "neckYaw", + "neckPitch", + ] + self.joint_goals = [ + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + float(0), + ] else: print(f"Unknown robot type {self.args.robot_type}") exit() if self.args.gazebo: - self.joint_publisher = self.create_publisher(Float64MultiArray, 'JointGroupController/command', 10) + self.joint_publisher = self.create_publisher(Float64MultiArray, "JointGroupController/command", 10) else: - self.joint_publisher = self.create_publisher(JointState, 'joint_states', 10) + self.joint_publisher = self.create_publisher(JointState, "joint_states", 10) if self.args.walking or self.args.all: self.create_subscription(JointCommand, "walking_motor_goals", self.joint_command_cb, 10) @@ -134,7 +207,7 @@ def get_float_array(self): return msg -if __name__ == '__main__': +if __name__ == "__main__": rclpy.init(args=sys.argv) node = MotorVizHelper() diff --git a/bitbots_utils/scripts/speak_ip.py b/bitbots_utils/scripts/speak_ip.py index c475d659..3e94888b 100755 --- a/bitbots_utils/scripts/speak_ip.py +++ b/bitbots_utils/scripts/speak_ip.py @@ -1,28 +1,27 @@ #!/usr/bin/env python3 -# -*- coding: utf8 -*- -from ipaddress import ip_address +import os import socket -from time import sleep + import pyttsx3 -import os -from netifaces import interfaces, ifaddresses, AF_INET +from netifaces import AF_INET, ifaddresses, interfaces ip_address = "not set" -for ifaceName in interfaces(): - addresses = [i['addr'] for i in ifaddresses(ifaceName).setdefault(AF_INET, [{'addr': 'No IP addr'}])] - if ifaceName=="wlp3s0": - ip_adress = ' '.join(addresses) + +for interface_name in interfaces(): + addresses = [i["addr"] for i in ifaddresses(interface_name).setdefault(AF_INET, [{"addr": "No IP addr"}])] + if interface_name == "wlp3s0": + ip_adress = " ".join(addresses) hostname = socket.gethostname() ip_parts = ip_adress.split(".") ip = ip_parts[0] + " point " + ip_parts[1] + " point " + ip_parts[2] + " point " + ip_parts[3] engine = pyttsx3.init() -engine.setProperty('rate', 175) +engine.setProperty("rate", 175) robot_name = os.getenv("ROBOT_NAME") -engine.say(f"Startup complete") +engine.say("Startup complete") engine.runAndWait() -engine.setProperty('rate', 175) +engine.setProperty("rate", 175) engine.say(f"{robot_name}") engine.say(f"My IP adress is {ip}") -engine.runAndWait() \ No newline at end of file +engine.runAndWait() diff --git a/bitbots_utils/scripts/tf_delay_plot.cpp b/bitbots_utils/scripts/tf_delay_plot.cpp index cb9fb6b7..23677441 100644 --- a/bitbots_utils/scripts/tf_delay_plot.cpp +++ b/bitbots_utils/scripts/tf_delay_plot.cpp @@ -1,54 +1,54 @@ -// This script logs the delay of certain tf frames and publishes them to be visualized in plotjuggler. +// This script logs the delay of certain tf frames and publishes them to be +// visualized in plotjuggler. -#include +#include +#include +#include #include +#include #include -#include -#include -#include -#include #include +#include using namespace std::placeholders; class TfDelayPlot : public rclcpp::Node { - - public: - TfDelayPlot() : Node("tf_delay_plot", rclcpp::NodeOptions()) { - RCLCPP_INFO(this->get_logger(), "Starting tf_delay_plot"); - // Create correct qos profile - tf_sub_ = this->create_subscription( - "/tf", - 5, - std::bind(&TfDelayPlot::tf_callback, this, _1)); - } - - void tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Got transform message"); - for (auto &transform: msg->transforms) { - // Calculate delay - double delay = ((transform.header.stamp.sec * 1.0e9 + transform.header.stamp.nanosec) - this->now().nanoseconds()) / 1.0e9; - - if (std::abs(delay) > 0.0001) { - // Print - RCLCPP_INFO(this->get_logger(), "Frame: %s, Delay: %f", transform.child_frame_id.c_str(), delay); - - // Print current time - RCLCPP_INFO(this->get_logger(), "Current time: %f", this->now().seconds()); - - // Print stamp - RCLCPP_INFO(this->get_logger(), "Stamp: %f", transform.header.stamp.sec + transform.header.stamp.nanosec / 1.0e9); - } - } - } - - private: - rclcpp::Subscription::SharedPtr tf_sub_; + public: + TfDelayPlot() : Node("tf_delay_plot", rclcpp::NodeOptions()) { + RCLCPP_INFO(this->get_logger(), "Starting tf_delay_plot"); + // Create correct qos profile + tf_sub_ = + this->create_subscription("/tf", 5, std::bind(&TfDelayPlot::tf_callback, this, _1)); + } + + void tf_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Got transform message"); + for (auto &transform : msg->transforms) { + // Calculate delay + double delay = + ((transform.header.stamp.sec * 1.0e9 + transform.header.stamp.nanosec) - this->now().nanoseconds()) / 1.0e9; + + if (std::abs(delay) > 0.0001) { + // Print + RCLCPP_INFO(this->get_logger(), "Frame: %s, Delay: %f", transform.child_frame_id.c_str(), delay); + + // Print current time + RCLCPP_INFO(this->get_logger(), "Current time: %f", this->now().seconds()); + + // Print stamp + RCLCPP_INFO(this->get_logger(), "Stamp: %f", + transform.header.stamp.sec + transform.header.stamp.nanosec / 1.0e9); + } + } + } + + private: + rclcpp::Subscription::SharedPtr tf_sub_; }; int main(int argc, char **argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/bitbots_utils/scripts/tf_monitor.py b/bitbots_utils/scripts/tf_monitor.py index c0ea1074..b9d90b30 100755 --- a/bitbots_utils/scripts/tf_monitor.py +++ b/bitbots_utils/scripts/tf_monitor.py @@ -1,10 +1,9 @@ #!/usr/bin/env python3 import argparse import time -from collections import deque, defaultdict +from collections import defaultdict, deque import rclpy -from rclpy.node import Node import tf2_ros from tf2_msgs.msg import TFMessage @@ -18,6 +17,7 @@ def __init__(self, use_chain, frame_a=None, frame_b=None): :param frame_a: The start frame of the monitored chain :param frame_b: The end frame of the monitored chain """ + def deque_1000(): """Construct a deque that stores at most 1000 items""" return deque(maxlen=1000) @@ -42,13 +42,13 @@ def deque_1000(): # Wait for the chain self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) - print(f'Waiting for transform chain to become available between {frame_a} and {frame_b}...') + print(f"Waiting for transform chain to become available between {frame_a} and {frame_b}...") while rclpy.ok(): - if self.tf_buffer.can_transform(frame_a, frame_b, rospy.Time()): + if self.tf_buffer.can_transform(frame_a, frame_b, rclpy.Time()): try: - self.chain = self.tf_buffer._chain(frame_b, rospy.Time(), frame_a, rospy.Time(), frame_b) + self.chain = self.tf_buffer._chain(frame_b, rclpy.Time(), frame_a, rclpy.Time(), frame_b) except tf2_ros.TransformException as e: - rospy.logwarn("Transform Exception", e) + rclpy.logwarn("Transform Exception", e) break else: time.sleep(0.1) @@ -63,9 +63,9 @@ def callback(self, msg: TFMessage, is_static): """ delay_sum = 0 update_chain = False - broadcaster = msg._connection_header['callerid'] + broadcaster = msg._connection_header["callerid"] if is_static: - broadcaster += ' (static)' + broadcaster += " (static)" for transform in msg.transforms: # Update the information about this transform @@ -87,7 +87,7 @@ def callback(self, msg: TFMessage, is_static): if update_chain and self.use_chain: # Update the chain delay - tmp = self.tf_buffer.lookup_transform(self.frame_a, self.frame_b, rospy.Time()) + tmp = self.tf_buffer.lookup_transform(self.frame_a, self.frame_b, rclpy.Time()) delay = (self.get_clock().now() - tmp.header.stamp).to_sec() self.chain_delay.append(delay) @@ -99,10 +99,10 @@ def display_frames(self, event=None): # This can happen when display_frames is called before the first tf callback return - print('\n\n') + print("\n\n") if event.last_real and event.current_real: - monitor_freq = round(1/(event.current_real - event.last_real).to_sec(), 3) - print(f'The tf_monitor is running with a display rate of {monitor_freq:.2f} Hz\n') + monitor_freq = round(1 / (event.current_real - event.last_real).to_sec(), 3) + print(f"The tf_monitor is running with a display rate of {monitor_freq:.2f} Hz\n") # For pretty-printing round_len = 10 @@ -118,48 +118,57 @@ def display_frames(self, event=None): if self.use_chain: # Print general information about the chain - print('Chain is:', ' -> '.join(self.chain)) + print("Chain is:", " -> ".join(self.chain)) average_delay = round(sum(self.chain_delay) / len(self.chain_delay), round_len) max_delay = round(max(self.chain_delay), round_len) - print(f'Average Net Delay: {average_delay:.{num_len}f} Max Net Delay: {max_delay:.{num_len}f}') - print('\nFrames in chain:') + print(f"Average Net Delay: {average_delay:.{num_len}f} Max Net Delay: {max_delay:.{num_len}f}") + print("\nFrames in chain:") else: - print('All Frames:') + print("All Frames:") # Print details about each frame for frame in sorted(frames): delay_list = self.frame_delay_dict[frame] average_delay = round(sum(delay_list) / len(delay_list), round_len) max_delay = round(max(delay_list), round_len) - print(f'Frame: {frame:{frame_len}} Published by {self.frame_broadcaster_dict[frame]:<{broadcaster_len}} ' - f'Average Delay: {average_delay:.{num_len}f} Max Delay: {max_delay:.{num_len}f}') + print( + f"Frame: {frame:{frame_len}} Published by {self.frame_broadcaster_dict[frame]:<{broadcaster_len}} " + f"Average Delay: {average_delay:.{num_len}f} Max Delay: {max_delay:.{num_len}f}" + ) # Print details about each broadcaster if self.use_chain: - print('\nBroadcasters in chain:') + print("\nBroadcasters in chain:") else: - print('\nAll Broadcasters:') + print("\nAll Broadcasters:") for node in sorted(broadcasters): - average_delay = round(sum(self.broadcaster_delay_dict[node]) / len(self.broadcaster_delay_dict[node]), round_len) + average_delay = round( + sum(self.broadcaster_delay_dict[node]) / len(self.broadcaster_delay_dict[node]), round_len + ) max_delay = round(max(self.broadcaster_delay_dict[node]), round_len) frequency_list = self.broadcaster_frequency_dict[node] frequency_out = round(len(frequency_list) / max(1e-8, (frequency_list[-1] - frequency_list[0])), round_len) - print(f'Node: {node:{broadcaster_len}} {frequency_out:.{num_len}f} Hz ' - f'Average Delay: {average_delay:.{num_len}f} Max Delay: {max_delay:.{num_len}f}') + print( + f"Node: {node:{broadcaster_len}} {frequency_out:.{num_len}f} Hz " + f"Average Delay: {average_delay:.{num_len}f} Max Delay: {max_delay:.{num_len}f}" + ) -if __name__ == '__main__': +if __name__ == "__main__": rclpy.init(args=None) - parser = argparse.ArgumentParser(usage='%(prog)s [-h] [--tf-topic TOPIC] [--display-rate HZ] [frame_a frame_b]\n', - description='Monitor the published tf messages. Without command line arguments, ' - 'monitor all frames and their publishers, with two arguments ' - 'monitor the chain between them.') - parser.add_argument('frame_a', nargs='?', help='The start frame of the monitored chain') - parser.add_argument('frame_b', nargs='?', help='The end frame of the monitored chain') - parser.add_argument('--tf-topic', required=False, default='tf', metavar='TOPIC', help='tf topic to listen to') - parser.add_argument('--display-rate', required=False, type=float, default=5.0, metavar='HZ', - help='display update rate') + parser = argparse.ArgumentParser( + usage="%(prog)s [-h] [--tf-topic TOPIC] [--display-rate HZ] [frame_a frame_b]\n", + description="Monitor the published tf messages. Without command line arguments, " + "monitor all frames and their publishers, with two arguments " + "monitor the chain between them.", + ) + parser.add_argument("frame_a", nargs="?", help="The start frame of the monitored chain") + parser.add_argument("frame_b", nargs="?", help="The end frame of the monitored chain") + parser.add_argument("--tf-topic", required=False, default="tf", metavar="TOPIC", help="tf topic to listen to") + parser.add_argument( + "--display-rate", required=False, type=float, default=5.0, metavar="HZ", help="display update rate" + ) args = parser.parse_args() if args.frame_a and not args.frame_b: @@ -167,16 +176,16 @@ def display_frames(self, event=None): use_chain = args.frame_a and args.frame_b - if args.tf_topic != 'tf' and use_chain: + if args.tf_topic != "tf" and use_chain: # This is not supported because it is not possible to change the tf topic for the ros tf listener - parser.error('Monitoring a specific chain is not supported with a custom tf topic') + parser.error("Monitoring a specific chain is not supported with a custom tf topic") - while self.get_clock().now() == rospy.Time() and rclpy.ok(): - rospy.loginfo_throttle(10, 'tf_monitor waiting for time to be published') + while rclpy.get_clock().now() == rclpy.Time() and rclpy.ok(): + rclpy.loginfo_throttle(10, "tf_monitor waiting for time to be published") time.sleep(0.1) monitor = TFMonitor(use_chain, args.frame_a, args.frame_b) - rospy.Subscriber(args.tf_topic, TFMessage, lambda msg: monitor.callback(msg, False)) - rospy.Subscriber(args.tf_topic + '_static', TFMessage, lambda msg: monitor.callback(msg, True)) - rospy.Timer(rospy.Duration(1/args.display_rate), monitor.display_frames) - rclpy.spin(self) + rclpy.Subscriber(args.tf_topic, TFMessage, lambda msg: monitor.callback(msg, False)) + rclpy.Subscriber(args.tf_topic + "_static", TFMessage, lambda msg: monitor.callback(msg, True)) + rclpy.Timer(rclpy.Duration(1 / args.display_rate), monitor.display_frames) + rclpy.spin() diff --git a/bitbots_utils/src/utils.cpp b/bitbots_utils/src/utils.cpp index f0c653a8..0cdbd33c 100644 --- a/bitbots_utils/src/utils.cpp +++ b/bitbots_utils/src/utils.cpp @@ -1,7 +1,6 @@ #include "bitbots_utils/utils.hpp" -namespace bitbots_utils -{ +namespace bitbots_utils { /** * @brief Waits for the transforms to be available @@ -11,31 +10,29 @@ namespace bitbots_utils * @param frames The tf frames to wait for * @param root_frame The root frame to transform from * @param check_interval Interval in which to check for the frames - * @param warn_duration Duration after which to warn if the frames are not available - * @param warn_interval Interval in which to keep warning if the frames are not available + * @param warn_duration Duration after which to warn if the frames are not + * available + * @param warn_interval Interval in which to keep warning if the frames are not + * available * @param verbose Can be used to disable the warning messages */ -void wait_for_tf( - const rclcpp::Logger &logger, - std::shared_ptr clock, - std::shared_ptr tf_buffer, - const std::vector &frames, - const std::string &root_frame, - const rclcpp::Duration &check_interval, - const rclcpp::Duration &warn_duration, - const rclcpp::Duration &warn_interval, - bool verbose) { - +void wait_for_tf(const rclcpp::Logger &logger, std::shared_ptr clock, + std::shared_ptr tf_buffer, const std::vector &frames, + const std::string &root_frame, const rclcpp::Duration &check_interval, + const rclcpp::Duration &warn_duration, const rclcpp::Duration &warn_interval, bool verbose) { // Store the beginning time auto start_time = clock->now(); // Endless loop - while(rclcpp::ok()) { + while (rclcpp::ok()) { try { // Check if the frame we want to transform is known yet - // Apply tf_buffer->_frameExists to all frames and check if all are available otherwise return false (functional) - // We use _frameExists because it does not spam the console with errors if the frame does not exist... - if(!std::all_of(frames.begin(), frames.end(), [tf_buffer](std::string frame) {return tf_buffer->_frameExists(frame);})) { + // Apply tf_buffer->_frameExists to all frames and check if all are + // available otherwise return false (functional) We use _frameExists + // because it does not spam the console with errors if the frame does not + // exist... + if (!std::all_of(frames.begin(), frames.end(), + [tf_buffer](std::string frame) { return tf_buffer->_frameExists(frame); })) { // Don't do busy waiting rclcpp::sleep_for(check_interval.to_chrono()); // Retry @@ -43,39 +40,29 @@ void wait_for_tf( } // Check if we can transform from the given root frame to all given frames - if(!std::all_of( - frames.begin(), - frames.end(), - [tf_buffer, root_frame, check_interval](const std::string frame) { - return tf_buffer->canTransform( - root_frame, - frame, - rclcpp::Time(0), - check_interval); - })){ - // Here it is fine not to wait as the canTransform function already includes a timeout - // Retry + if (!std::all_of(frames.begin(), frames.end(), [tf_buffer, root_frame, check_interval](const std::string frame) { + return tf_buffer->canTransform(root_frame, frame, rclcpp::Time(0), check_interval); + })) { + // Here it is fine not to wait as the canTransform function already + // includes a timeout Retry continue; } // We can transform to all frames, so we are done return; - } - catch(const std::exception& e) - { + } catch (const std::exception &e) { if (verbose) { RCLCPP_ERROR(logger, "Error while waiting for transforms: %s \n", e.what()); } rclcpp::sleep_for(check_interval.to_chrono()); } - + // Print error message if we waited too long auto wait_time = clock->now() - start_time; if (verbose && wait_time > warn_duration) { RCLCPP_WARN_THROTTLE(logger, *clock, warn_interval.seconds(), - "Waiting for transforms took longer than %f seconds", - wait_time.seconds()); + "Waiting for transforms took longer than %f seconds", wait_time.seconds()); } } } -} +} // namespace bitbots_utils diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 00000000..3ed6ff7a --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,14 @@ +[tool.black] +line-length = 120 + +[tool.ruff] +line-length = 120 + # Never enforce `E501` (line length violations), as black takes care of this. +ignore = ["E501"] +# Additionally enable the following rules +# - pycodestyle warnings (`W`) +# - flake8-bugbear warnings (`B`) +# - isort import sorting (`I`) +# - pep8-naming convenrtions (`N`) +# - pyupgrade prefer newer language constructs (`UP`) +select = ["F", "E", "B", "W", "I", "N", "UP"] diff --git a/system_monitor/setup.py b/system_monitor/setup.py index fbe93b00..7a0ca609 100644 --- a/system_monitor/setup.py +++ b/system_monitor/setup.py @@ -1,27 +1,24 @@ import glob -from setuptools import setup, find_packages +from setuptools import find_packages, setup -package_name = 'system_monitor' +package_name = "system_monitor" setup( name=package_name, packages=find_packages(), data_files=[ - ('share/' + package_name, ['package.xml']), - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name + "/config", - glob.glob('config/*.yaml')), - ('share/' + package_name + '/launch', - glob.glob('launch/*.launch')), + ("share/" + package_name, ["package.xml"]), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name + "/config", glob.glob("config/*.yaml")), + ("share/" + package_name + "/launch", glob.glob("launch/*.launch")), ], install_requires=[ - 'setuptools', + "setuptools", ], entry_points={ - 'console_scripts': [ - 'monitor = system_monitor.monitor:main', + "console_scripts": [ + "monitor = system_monitor.monitor:main", ], - } -) \ No newline at end of file + }, +) diff --git a/system_monitor/system_monitor/cpus.py b/system_monitor/system_monitor/cpus.py index 48c793bc..b0d3d9a8 100644 --- a/system_monitor/system_monitor/cpus.py +++ b/system_monitor/system_monitor/cpus.py @@ -1,6 +1,6 @@ from collections import defaultdict -import psutil +import psutil from bitbots_msgs.msg import Cpu as CpuMsg _prev_total = defaultdict(int) @@ -15,23 +15,20 @@ def collect_all(): """ msgs = [] running_processes = len(psutil.pids()) - timings = _get_cpu_stats() + timing_stats = _get_cpu_stats() overall_usage = 0 - for cpu, timings in timings.items(): + for cpu, timings in timing_stats.items(): cpu_total = sum(timings) del timings[3:5] cpu_busy = sum(timings) cpu_usage = _calculate_usage(cpu, cpu_total, cpu_busy) - msgs.append(CpuMsg( - cpu_name=cpu, - cpu_usage=cpu_usage - )) + msgs.append(CpuMsg(cpu_name=cpu, cpu_usage=cpu_usage)) overall_usage += cpu_usage # compute mean of cpu usages - overall_usage_percentage = overall_usage / len(timings) + overall_usage_percentage = overall_usage / len(timing_stats) return running_processes, msgs, overall_usage_percentage @@ -41,10 +38,10 @@ def _get_cpu_stats(): :returns timings which contains accumulative busy and total cpu time """ timings = {} - with open('/proc/stat', 'r') as file_obj: + with open("/proc/stat") as file_obj: for line in file_obj: # only evaluate lines like cpu0, cpu1, cpu2, ... - if line.startswith('cpu') and line.strip().split()[0] != 'cpu': + if line.startswith("cpu") and line.strip().split()[0] != "cpu": line = line.strip().split() timings[line[0]] = [int(x) for x in line[1:]] diff --git a/system_monitor/system_monitor/monitor.py b/system_monitor/system_monitor/monitor.py index ca52385c..7b7c17a8 100755 --- a/system_monitor/system_monitor/monitor.py +++ b/system_monitor/system_monitor/monitor.py @@ -1,19 +1,20 @@ #!/usr/bin/env python3 import socket +import time + import rclpy +from bitbots_msgs.msg import Workload as WorkloadMsg +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus from rclpy.node import Node -from bitbots_msgs.msg import Workload as WorkloadMsg from system_monitor import cpus, memory, network_interfaces -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -import time def main(): rclpy.init(args=None) - node = Node('system_monitor') - pub = node.create_publisher(WorkloadMsg, 'system_workload', 1) - diagnostic_pub = node.create_publisher(DiagnosticArray, 'diagnostics', 1) + node = Node("system_monitor") + pub = node.create_publisher(WorkloadMsg, "system_workload", 1) + diagnostic_pub = node.create_publisher(DiagnosticArray, "diagnostics", 1) hostname = socket.gethostname() @@ -26,27 +27,25 @@ def main(): diag_mem.name = "SYSTEMMemory" diag_cpu.hardware_id = "Memory" - node.declare_parameter('update_frequency', 10.0) - node.declare_parameter('do_memory', True) - node.declare_parameter('do_cpu', True) - node.declare_parameter('cpu_load_percentage', 80.0) - node.declare_parameter('memoroy_load_percentage', 80.0) - node.declare_parameter('network_rate_received_errors', 10.0) - node.declare_parameter('network_rate_send_errors', 10.0) + node.declare_parameter("update_frequency", 10.0) + node.declare_parameter("do_memory", True) + node.declare_parameter("do_cpu", True) + node.declare_parameter("cpu_load_percentage", 80.0) + node.declare_parameter("memoroy_load_percentage", 80.0) + node.declare_parameter("network_rate_received_errors", 10.0) + node.declare_parameter("network_rate_send_errors", 10.0) - rate = node.get_parameter('update_frequency').get_parameter_value().double_value - do_memory = node.get_parameter('do_memory').get_parameter_value().bool_value - do_cpu = node.get_parameter('do_cpu').get_parameter_value().bool_value - cpu_load_percentage = node.get_parameter('cpu_load_percentage').get_parameter_value().double_value - memoroy_load_percentage = node.get_parameter('memoroy_load_percentage').get_parameter_value().double_value - network_rate_received_errors = node.get_parameter( - 'network_rate_received_errors').get_parameter_value().double_value - network_rate_send_errors = node.get_parameter('network_rate_send_errors').get_parameter_value().double_value + rate = node.get_parameter("update_frequency").get_parameter_value().double_value + do_memory = node.get_parameter("do_memory").get_parameter_value().bool_value + do_cpu = node.get_parameter("do_cpu").get_parameter_value().bool_value + cpu_load_percentage = node.get_parameter("cpu_load_percentage").get_parameter_value().double_value + memoroy_load_percentage = node.get_parameter("memoroy_load_percentage").get_parameter_value().double_value + network_rate_received_errors = node.get_parameter("network_rate_received_errors").get_parameter_value().double_value + network_rate_send_errors = node.get_parameter("network_rate_send_errors").get_parameter_value().double_value while rclpy.ok(): last_send_time = time.time() - running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else ( - -1, [], 0) + running_processes, cpu_usages, overall_usage_percentage = cpus.collect_all() if do_cpu else (-1, [], 0) memory_available, memory_used, memory_total = memory.collect_all() if do_memory else (-1, -1, -1) interfaces = network_interfaces.collect_all(node.get_clock()) @@ -59,7 +58,7 @@ def main(): memory_used=memory_used, memory_total=memory_total, filesystems=[], - network_interfaces=interfaces + network_interfaces=interfaces, ) pub.publish(msg) @@ -84,8 +83,10 @@ def main(): diag_net = DiagnosticStatus() diag_net.name = "SYSTEM" + interface.name diag_net.hardware_id = interface.name - if interface.rate_received_packets_errors >= network_rate_received_errors \ - or interface.rate_sent_packets_errors >= network_rate_send_errors: + if ( + interface.rate_received_packets_errors >= network_rate_received_errors + or interface.rate_sent_packets_errors >= network_rate_send_errors + ): diag_net.level = DiagnosticStatus.WARN else: diag_net.level = DiagnosticStatus.OK diff --git a/system_monitor/system_monitor/network_interfaces.py b/system_monitor/system_monitor/network_interfaces.py index d602663a..b896bef1 100644 --- a/system_monitor/system_monitor/network_interfaces.py +++ b/system_monitor/system_monitor/network_interfaces.py @@ -1,7 +1,6 @@ from collections import defaultdict + from bitbots_msgs.msg import NetworkInterface as NetworkInterfaceMsg -from rclpy.duration import Duration -from rclpy.time import Time _prev_msgs = defaultdict(NetworkInterfaceMsg) _prev_msg_time = None @@ -19,12 +18,12 @@ def collect_all(clock): def _get_interfaces(): """@rtype: dict""" result = {} - with open('/proc/net/dev') as file_obj: + with open("/proc/net/dev") as file_obj: for line in file_obj: - if '|' not in line: # exclude table header lines + if "|" not in line: # exclude table header lines line = line.strip().split() - name = line[0].replace(':', '') + name = line[0].replace(":", "") msg = NetworkInterfaceMsg( name=name, received_bytes=int(line[1]), @@ -51,15 +50,43 @@ def _analyze_rate(msgs, clock): if interface not in _prev_msgs: continue - i_msg.rate_received_bytes = int((i_msg.received_bytes - _prev_msgs[interface].received_bytes) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_received_packets = int((i_msg.received_packets - _prev_msgs[interface].received_packets) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_received_packets_errors = int((i_msg.received_packets_errors - _prev_msgs[interface].received_packets_errors) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_received_packets_dropped = int((i_msg.received_packets_dropped - _prev_msgs[interface].received_packets_dropped) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_sent_bytes = int((i_msg.sent_bytes - _prev_msgs[interface].sent_bytes) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_sent_packets = int((i_msg.sent_packets - _prev_msgs[interface].sent_packets) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_sent_packets_errors = int((i_msg.sent_packets_errors - _prev_msgs[interface].sent_packets_errors) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_sent_packets_dropped = int((i_msg.sent_packets_dropped - _prev_msgs[interface].sent_packets_dropped) * (now - _prev_msg_time).nanoseconds/1e9) - i_msg.rate_sent_packets_collisions = int((i_msg.sent_packets_collisions - _prev_msgs[interface].sent_packets_collisions) * (now - _prev_msg_time).nanoseconds/1e9) + i_msg.rate_received_bytes = int( + (i_msg.received_bytes - _prev_msgs[interface].received_bytes) * (now - _prev_msg_time).nanoseconds / 1e9 + ) + i_msg.rate_received_packets = int( + (i_msg.received_packets - _prev_msgs[interface].received_packets) * (now - _prev_msg_time).nanoseconds / 1e9 + ) + i_msg.rate_received_packets_errors = int( + (i_msg.received_packets_errors - _prev_msgs[interface].received_packets_errors) + * (now - _prev_msg_time).nanoseconds + / 1e9 + ) + i_msg.rate_received_packets_dropped = int( + (i_msg.received_packets_dropped - _prev_msgs[interface].received_packets_dropped) + * (now - _prev_msg_time).nanoseconds + / 1e9 + ) + i_msg.rate_sent_bytes = int( + (i_msg.sent_bytes - _prev_msgs[interface].sent_bytes) * (now - _prev_msg_time).nanoseconds / 1e9 + ) + i_msg.rate_sent_packets = int( + (i_msg.sent_packets - _prev_msgs[interface].sent_packets) * (now - _prev_msg_time).nanoseconds / 1e9 + ) + i_msg.rate_sent_packets_errors = int( + (i_msg.sent_packets_errors - _prev_msgs[interface].sent_packets_errors) + * (now - _prev_msg_time).nanoseconds + / 1e9 + ) + i_msg.rate_sent_packets_dropped = int( + (i_msg.sent_packets_dropped - _prev_msgs[interface].sent_packets_dropped) + * (now - _prev_msg_time).nanoseconds + / 1e9 + ) + i_msg.rate_sent_packets_collisions = int( + (i_msg.sent_packets_collisions - _prev_msgs[interface].sent_packets_collisions) + * (now - _prev_msg_time).nanoseconds + / 1e9 + ) _prev_msg_time = now _prev_msgs = msgs