Add voxelmap, front camera and TTS

.gitignore

@@ -0,0 +1,171 @@
+.DS_Store
+__pycache__/
+*.py[cod]
+*$py.class
+
+# C extensions
+*.so
+
+# Distribution / packaging
+.Python
+build/
+develop-eggs/
+dist/
+downloads/
+eggs/
+.eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+wheels/
+share/python-wheels/
+*.egg-info/
+.installed.cfg
+*.egg
+MANIFEST
+
+# PyInstaller
+#  Usually these files are written by a python script from a template
+#  before PyInstaller builds the exe, so as to inject date/other infos into it.
+*.manifest
+*.spec
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+htmlcov/
+.tox/
+.nox/
+.coverage
+.coverage.*
+.cache
+nosetests.xml
+coverage.xml
+*.cover
+*.py,cover
+.hypothesis/
+.pytest_cache/
+cover/
+
+# Translations
+*.mo
+*.pot
+
+# Django stuff:
+*.log
+local_settings.py
+db.sqlite3
+db.sqlite3-journal
+
+# Flask stuff:
+instance/
+.webassets-cache
+
+# Scrapy stuff:
+.scrapy
+
+# Sphinx documentation
+docs/_build/
+
+# PyBuilder
+.pybuilder/
+target/
+
+# Jupyter Notebook
+.ipynb_checkpoints
+
+# IPython
+profile_default/
+ipython_config.py
+
+# pyenv
+#   For a library or package, you might want to ignore these files since the code is
+#   intended to run in multiple environments; otherwise, check them in:
+# .python-version
+
+# pipenv
+#   According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.
+#   However, in case of collaboration, if having platform-specific dependencies or dependencies
+#   having no cross-platform support, pipenv may install dependencies that don't work, or not
+#   install all needed dependencies.
+#Pipfile.lock
+
+# UV
+#   Similar to Pipfile.lock, it is generally recommended to include uv.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#uv.lock
+
+# poetry
+#   Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control.
+#   This is especially recommended for binary packages to ensure reproducibility, and is more
+#   commonly ignored for libraries.
+#   https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control
+#poetry.lock
+
+# pdm
+#   Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control.
+#pdm.lock
+#   pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it
+#   in version control.
+#   https://pdm.fming.dev/latest/usage/project/#working-with-version-control
+.pdm.toml
+.pdm-python
+.pdm-build/
+
+# PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm
+__pypackages__/
+
+# Celery stuff
+celerybeat-schedule
+celerybeat.pid
+
+# SageMath parsed files
+*.sage.py
+
+# Environments
+.env
+.venv
+env/
+venv/
+ENV/
+env.bak/
+venv.bak/
+
+# Spyder project settings
+.spyderproject
+.spyproject
+
+# Rope project settings
+.ropeproject
+
+# mkdocs documentation
+/site
+
+# mypy
+.mypy_cache/
+.dmypy.json
+dmypy.json
+
+# Pyre type checker
+.pyre/
+
+# pytype static type analyzer
+.pytype/
+
+# Cython debug symbols
+cython_debug/
+
+# PyCharm
+#  JetBrains specific template is maintained in a separate JetBrains.gitignore that can
+#  be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore
+#  and can be added to the global gitignore or merged into this file.  For a more nuclear
+#  option (not recommended) you can uncomment the following to ignore the entire idea folder.
+#.idea/
+
+# PyPI configuration file
+.pypirc

go2_bringup/launch/go2.launch.py

@@ -30,18 +30,21 @@
 
 import os
 
-from ament_index_python.packages import get_package_share_directory
+from ament_index_python.packages import get_package_share_directory, PackageNotFoundError
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, LogInfo
 from launch.conditions import IfCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
 
 
 def generate_launch_description():
     lidar = LaunchConfiguration('lidar')
     realsense = LaunchConfiguration('realsense')
     rviz = LaunchConfiguration('rviz')
+    voxelmap = LaunchConfiguration('voxelmap')
+    video = LaunchConfiguration('video')
 
     declare_lidar_cmd = DeclareLaunchArgument(
         'lidar',
@@ -61,6 +64,18 @@ def generate_launch_description():
         description='Launch rviz'
     )
 
+    declare_voxelmap_cmd = DeclareLaunchArgument(
+        'voxelmap',
+        default_value='False',
+        description='Generate voxel map'
+    )
+
+    declare_video_cmd = DeclareLaunchArgument(
+        'video',
+        default_value='True',
+        description='Publish front camera video'
+    )
+
     robot_description_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([os.path.join(
             get_package_share_directory('go2_description'),
@@ -73,33 +88,66 @@ def generate_launch_description():
             'launch/'), 'go2_driver.launch.py'])
     )
 
-    lidar_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(
-            get_package_share_directory('hesai_ros_driver'),
-            'launch/'), 'start.py']),
-        condition=IfCondition(PythonExpression([lidar]))
+    voxelmap_node_cmd = Node(
+        package='go2_voxelmap',
+        executable='voxelmap_node',
+        name='voxelmap_node',
+        condition=IfCondition(PythonExpression([voxelmap])),
+        output='screen'
     )
 
-    realsense_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(
-            get_package_share_directory('realsense2_camera'),
-            'launch/'), 'rs_launch.py']),
-        condition=IfCondition(PythonExpression([realsense]))
+    video_node_cmd = Node(
+        package='image_transport',
+        executable='republish',
+        name='image_republish_node',
+        arguments=['go2', 'compressed'],
+        output='screen',
+        condition=IfCondition(PythonExpression([video])),
+        remappings=[
+            ('in/go2', 'frontvideostream'),
+            ('out/compressed', 'camera/compressed')
+        ]
     )
 
-    rviz_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(
-            get_package_share_directory('go2_rviz'),
-            'launch/'), 'rviz.launch.py']),
-        condition=IfCondition(PythonExpression([rviz]))
-    )
+    try:
+        lidar_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('hesai_ros_driver'),
+                'launch/'), 'start.py']),
+            condition=IfCondition(PythonExpression([lidar])))
+    except PackageNotFoundError:
+        lidar_cmd = LogInfo(msg="hesai_ros_driver package not found. Skipping LiDAR launch.")
+
+    try:
+        realsense_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('realsense2_camera'),
+                'launch/'), 'rs_launch.py']),
+            condition=IfCondition(PythonExpression([realsense]))
+        )
+    except PackageNotFoundError:
+        realsense_cmd = LogInfo(msg="realsense2_camera package not found. Skipping LiDAR launch.")
+
+    try:
+        rviz_cmd = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([os.path.join(
+                get_package_share_directory('go2_rviz'),
+                'launch/'), 'rviz.launch.py']),
+            condition=IfCondition(PythonExpression([rviz]))
+        )
+    except PackageNotFoundError:
+        rviz_cmd = LogInfo(msg="no rviz package found. Skipping rviz launch.")
 
     ld = LaunchDescription()
     ld.add_action(declare_lidar_cmd)
     ld.add_action(declare_realsense_cmd)
     ld.add_action(declare_rviz_cmd)
+    ld.add_action(declare_video_cmd)
+    ld.add_action(declare_voxelmap_cmd)
     ld.add_action(robot_description_cmd)
     ld.add_action(lidar_cmd)
+    ld.add_action(voxelmap_node_cmd)
+    ld.add_action(video_node_cmd)
     ld.add_action(realsense_cmd)
     ld.add_action(driver_cmd)
     ld.add_action(rviz_cmd)

go2_image_transport/CMakeLists.txt

@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.12.2)
+
+project(go2_image_transport)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  message(STATUS "GNU or Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# Find ffmpeg libraries
+find_package(PkgConfig)
+pkg_check_modules(PC_AVDEVICE REQUIRED libavdevice>=58)
+pkg_check_modules(PC_AVFORMAT REQUIRED libavformat>=58)
+pkg_check_modules(PC_AVCODEC REQUIRED libavcodec>=58)
+pkg_check_modules(PC_AVUTIL REQUIRED libavutil>=56)
+pkg_check_modules(PC_SWSCALE REQUIRED libswscale>=5)
+
+ament_auto_add_library(
+  ${PROJECT_NAME} SHARED
+  src/go2_subscriber.cpp
+  src/manifest.cpp
+)
+
+target_link_libraries(
+  ${PROJECT_NAME}
+  ${PC_AVDEVICE_LIBRARIES}
+  ${PC_AVFORMAT_LIBRARIES}
+  ${PC_AVCODEC_LIBRARIES}
+  ${PC_AVUTIL_LIBRARIES}
+  ${PC_SWSCALE_LIBRARIES}
+)
+
+# This will load & run linters listed in package.xml
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(
+  DIRECTORY include
+  DESTINATION include
+)
+
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+pluginlib_export_plugin_description_file(image_transport go2_plugins.xml)
+
+ament_auto_package()

go2_image_transport/README.md

@@ -0,0 +1,4 @@
+Image transport plugin to decode Unitree Go2 '/frontcamera' messages
+
+
+This project build on code from `h264_image_transport` by Clyde McQueen , which is licensed under the BSD 3-Clause License.

go2_image_transport/go2_plugins.xml

@@ -0,0 +1,10 @@
+<library path="go2_image_transport">
+    <class
+            name="image_transport/go2_sub"
+            type="go2_image_transport::Go2Subscriber"
+            base_class_type="image_transport::SubscriberPlugin">
+        <description>
+            Decodes Go2's h264 video stream.
+        </description>
+    </class>
+</library>