diff --git a/.gitignore b/.gitignore index 8f22ba6..d9a35e9 100644 --- a/.gitignore +++ b/.gitignore @@ -3,6 +3,6 @@ log build .idea src/ros2cs -**/metadata*.xml src/Ros2ForUnity/Plugins !src/Ros2ForUnity/Plugins/.gitkeep +src/Ros2ForUnity/Resources/ros2-for-unity.xml diff --git a/README-UBUNTU.md b/README-UBUNTU.md index a700815..eb73f68 100644 --- a/README-UBUNTU.md +++ b/README-UBUNTU.md @@ -40,12 +40,7 @@ Start with installation of dependencies. Make sure to complete each step of `ros ./build.sh ``` * You can add `--clean-install` flag to make sure your installation directory is cleaned before deploying. -* Unity Asset is ready to import into your Unity project. You can find it in `install/asset/` directory. -* (optionally) To create `.unitypackage` in `install/unity_package` - ```bash - create_unity_package.sh -u - ``` - > *NOTE* Unity license is required. +* Unity package is ready to import into your Unity project. You can find it in `install/package/` directory. ## OS-Specific usage remarks diff --git a/README-WINDOWS.md b/README-WINDOWS.md index f501fd0..9893a7e 100644 --- a/README-WINDOWS.md +++ b/README-WINDOWS.md @@ -42,12 +42,7 @@ It is necessary to complete all the steps for `ros2cs` [Prerequisites](https://g ./build.ps1 ``` * You can build with `-clean_install` to make sure your installation directory is cleaned before deploying. -* Unity Asset is ready to import into your Unity project. You can find it in `install/asset/` directory. -* (optionally) To create `.unitypackage` in `install/unity_package` - ```powershell - create_unity_package.ps1 - ``` - > *NOTE* Please provide path to your Unity executable when prompted. Unity license is required. In case your Unity license has expired, the `create_unity_package.ps1` won't throw any errors but `Ros2ForUnity.unitypackage` won't be generated too. +* Unity package is ready to import into your Unity project. You can find it in `install/package/` directory. ## Build troubleshooting @@ -85,9 +80,7 @@ pip install numpy ## OS-Specific usage remarks -> If the Asset is built with `-standalone` flag (the default), then nothing extra needs to be done. +> If the package is built with `-standalone` flag (the default), then nothing extra needs to be done. Otherwise, you have to source your ros distribution before launching either Unity3D Editor or Application. -> Note that after you build the Asset, you can use it on a machine that has no ros2 installation (if built with `-standalone`). - -> You can simply copy over the `Ros2ForUnity` subdirectory to update your Asset. +> Note that after you build the package, you can use it on a machine that has no ros2 installation (if built with `-standalone`). diff --git a/README.md b/README.md index 11e84f0..2be44e6 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ Advantages of this module include: - The module supplies abstractions and tools to use in your Unity project, including transformations, sensor interface, a clock, spinning loop wrapped in a MonoBehavior, handling initialization and shutdown. - Supports all standard ROS2 messages. - Custom messages are generated automatically with build, using standard ROS2 way. It is straightforward to generate and use them without having to define `.cs` equivalents by hand. -- The module is wrapped as a Unity asset. +- The module is wrapped as a Unity package. ## Platforms @@ -44,7 +44,7 @@ You can download pre-built [releases](https://github.com/RobotecAI/ros2-for-unit ## Building > **Note:** The project will pull `ros2cs` into the workspace, which also functions independently as it is a more general project aimed at any `C# / .Net` environment. -It has its own README and scripting, but for building the Unity Asset, please use instructions and scripting in this document instead, unless you also wish to run tests or examples for `ros2cs`. +It has its own README and scripting, but for building the Unity package, please use instructions and scripting in this document instead, unless you also wish to run tests or examples for `ros2cs`. Please see OS-specific instructions: - [Instructions for Ubuntu](README-UBUNTU.md) @@ -59,16 +59,18 @@ Custom messages can be included in the build by either: ## Installation 1. Perform building steps described in the OS-specific readme or download pre-built Unity package. Do not source `ros2-for-unity` nor `ros2cs` project into ROS2 workspace. -1. Open or create Unity project. -1. Import asset into project: - 1. copy `install/asset/Ros2ForUnity` into your project `Assets` folder, or - 1. if you have deployed an `.unitypackage` - import it in Unity Editor by selecting `Import Package` → `Custom Package` +2. Open or create Unity project +3. Import `install/package/Ros2ForUnity` into your project with the package manager + +**Before uploading the package into a registry you have to import it once from disk to generate .meta files for the native libraries!** ## Usage **Prerequisites** -* If your build was prepared with `--standalone` flag then you are fine, and all you have to do is run the editor +* When running in edit mode call `Setup.SetupPath()` to prevent the native libraries from failing to load. + +* If your build was prepared with `--standalone` flag then you are fine, and all you have to do is calling `Setup.SetupPath()` when necessary otherwise @@ -77,20 +79,20 @@ otherwise **Initializing Ros2ForUnity** 1. Initialize `Ros2ForUnity` by creating a "hook" object which will be your wrapper around ROS2. You have two options: - 1. `ROS2UnityComponent` based on `MonoBehaviour` which must be attached to a `GameObject` somewhere in the scene, then: + - `ROS2UnityComponent` based on `MonoBehaviour` which must be attached to a `GameObject` somewhere in the scene, then: ```c# using ROS2; ... // Example method of getting component, if ROS2UnityComponent lives in different GameObject, just use different get component methods. ROS2UnityComponent ros2Unity = GetComponent(); ``` - 1. or `ROS2UnityCore` which is a standard class that can be created anywhere + - `ROS2UnityCore` which is a standard class that can be created anywhere ```c# using ROS2; ... ROS2UnityCore ros2Unity = new ROS2UnityCore(); ``` -1. Create a node. You must first check if `Ros2ForUnity` is initialized correctly: +2. Create a node. You must first check if `Ros2ForUnity` is initialized correctly: ```c# private ROS2Node ros2Node; ... @@ -173,20 +175,7 @@ otherwise ``` ### Examples -1. Create a top-level object containing `ROS2UnityComponent.cs`. This is the central `Monobehavior` for `Ros2ForUnity` that manages all the nodes. Refer to class documentation for details. - > **Note:** Each example script looks for `ROS2UnityComponent` in its own game object. However, this is not a requirement, just example implementation. - -**Topics** -1. Add `ROS2TalkerExample.cs` script to the very same game object. -1. Add `ROS2ListenerExample.cs` script to the very same game object. - -Once you start the project in Unity, you should be able to see two nodes talking with each other in Unity Editor's console or use `ros2 node list` and `ros2 topic echo /chatter` to verify ros2 communication. - -**Services** -1. Add `ROS2ServiceExample.cs` script to the very same game object. -1. Add `ROS2ClientExample.cs` script to the very same game object. - -Once you start the project in Unity, you should be able to see client node calling an example service. +More complete examples are included in the Unity package. ## Acknowledgements diff --git a/build.ps1 b/build.ps1 index aa68889..bac5fca 100644 --- a/build.ps1 +++ b/build.ps1 @@ -12,14 +12,14 @@ Makes a clean installation. Removes install dir before deploying #> Param ( - [Parameter(Mandatory=$false)][switch]$with_tests=$false, - [Parameter(Mandatory=$false)][switch]$standalone=$false, - [Parameter(Mandatory=$false)][switch]$clean_install=$false + [Parameter(Mandatory = $false)][switch]$with_tests = $false, + [Parameter(Mandatory = $false)][switch]$standalone = $false, + [Parameter(Mandatory = $false)][switch]$clean_install = $false ) $scriptPath = split-path -parent $MyInvocation.MyCommand.Definition -if(-Not (Test-Path -Path "$scriptPath\src\ros2cs")) { +if (-Not (Test-Path -Path "$scriptPath\src\ros2cs")) { Write-Host "Pull repositories with 'pull_repositories.ps1' first." -ForegroundColor Red exit 1 } @@ -30,31 +30,32 @@ $options = @{ standalone = $standalone } -if($clean_install) { +if ($clean_install) { Write-Host "Cleaning install directory..." -ForegroundColor White Remove-Item -Path "$scriptPath\install" -Force -Recurse -ErrorAction Ignore } -if($standalone) { - & "python" $SCRIPTPATH\src\scripts\metadata_generator.py --standalone -} else { - & "python" $SCRIPTPATH\src\scripts\metadata_generator.py +if ($standalone) { + & "python" $SCRIPTPATH\src\scripts\metadata_generator.py --standalone +} +else { + & "python" $SCRIPTPATH\src\scripts\metadata_generator.py +} +if ($LASTEXITCODE -ne 0) { + Write-Host "Generating the Metadata failed!" -ForegroundColor Red + exit 1 } & "$scriptPath\src\ros2cs\build.ps1" @options -if($?) { - md -Force $scriptPath\install\asset | Out-Null - Copy-Item -Path $scriptPath\src\Ros2ForUnity -Destination $scriptPath\install\asset\ -Recurse -Force +if ($?) { + mkdir -Force $scriptPath\install\package | Out-Null + Copy-Item -Path $scriptPath\src\Ros2ForUnity -Destination $scriptPath\install\package\ -Recurse -Force - $plugin_path=Join-Path -Path $scriptPath -ChildPath "\install\asset\Ros2ForUnity\Plugins\" + $plugin_path = Join-Path -Path $scriptPath -ChildPath "\install\package\Ros2ForUnity\Plugins\" Write-Host "Deploying build to $plugin_path" -ForegroundColor Green & "$scriptPath\deploy_unity_plugins.ps1" $plugin_path - - Copy-Item -Path $scriptPath\src\Ros2ForUnity\metadata_ros2cs.xml -Destination $scriptPath\install\asset\Ros2ForUnity\Plugins\Windows\x86_64\ - Copy-Item -Path $scriptPath\src\Ros2ForUnity\metadata_ros2cs.xml -Destination $scriptPath\install\asset\Ros2ForUnity\Plugins\ -} else { +} +else { Write-Host "Ros2cs build failed!" -ForegroundColor Red exit 1 } - - diff --git a/build.sh b/build.sh index bef53d5..b709e95 100755 --- a/build.sh +++ b/build.sh @@ -1,6 +1,6 @@ -#!/bin/bash -SCRIPT=$(readlink -f $0) -SCRIPTPATH=`dirname $SCRIPT` +#!/bin/sh -e +SCRIPT=$(readlink -f "$0") +SCRIPTPATH=$(dirname "$SCRIPT") display_usage() { echo "Usage: " @@ -23,7 +23,7 @@ STANDALONE=0 TESTS=0 CLEAN_INSTALL=0 -while [[ $# -gt 0 ]]; do +while [ $# -gt 0 ]; do key="$1" case $key in -t|--with-tests) @@ -47,7 +47,6 @@ while [[ $# -gt 0 ]]; do -h|--help) display_usage exit 0 - shift # past argument ;; *) # unknown option shift # past argument @@ -55,22 +54,20 @@ while [[ $# -gt 0 ]]; do esac done -if [ $CLEAN_INSTALL == 1 ]; then +if [ $CLEAN_INSTALL = 1 ]; then echo "Cleaning install directory..." - rm -rf $SCRIPTPATH/install/* + rm -rf "${SCRIPTPATH}/install/"* fi -if [ $STANDALONE == 1 ]; then - python3 $SCRIPTPATH/src/scripts/metadata_generator.py --standalone +if [ $STANDALONE = 1 ]; then + python3 "${SCRIPTPATH}/src/scripts/metadata_generator.py" --standalone else - python3 $SCRIPTPATH/src/scripts/metadata_generator.py + python3 "${SCRIPTPATH}/src/scripts/metadata_generator.py" fi -if $SCRIPTPATH/src/ros2cs/build.sh $OPTIONS; then - mkdir -p $SCRIPTPATH/install/asset && cp -R $SCRIPTPATH/src/Ros2ForUnity $SCRIPTPATH/install/asset/ - $SCRIPTPATH/deploy_unity_plugins.sh $SCRIPTPATH/install/asset/Ros2ForUnity/Plugins/ - cp $SCRIPTPATH/src/Ros2ForUnity/metadata_ros2cs.xml $SCRIPTPATH/install/asset/Ros2ForUnity/Plugins/Linux/x86_64/metadata_ros2cs.xml - cp $SCRIPTPATH/src/Ros2ForUnity/metadata_ros2cs.xml $SCRIPTPATH/install/asset/Ros2ForUnity/Plugins/metadata_ros2cs.xml +if "${SCRIPTPATH}/src/ros2cs/build.sh" $OPTIONS; then + mkdir -p "${SCRIPTPATH}/install/package" && cp -R "${SCRIPTPATH}/src/Ros2ForUnity" "${SCRIPTPATH}/install/package/" + "${SCRIPTPATH}/deploy_unity_plugins.sh" "${SCRIPTPATH}/install/package/Ros2ForUnity/Plugins/" else echo "Ros2cs build failed!" exit 1 diff --git a/create_unity_package.ps1 b/create_unity_package.ps1 deleted file mode 100644 index 3eacba2..0000000 --- a/create_unity_package.ps1 +++ /dev/null @@ -1,85 +0,0 @@ - -<# -.SYNOPSIS - Creates a 'unitypackage' from an input asset. -.DESCRIPTION - This script screates a temporary Unity project in "%USERPROFILE%\AppData\Local\Temp" directory, copy input asset and makes an unity package out of it. Valid Unity license is required. -.PARAMETER unity_path - Unity editor executable path -.PARAMETER input_asset - input asset to pack into unity package -.PARAMETER package_name - Unity package name -.PARAMETER output_dir - output file directory -#> -Param ( - [Parameter(Mandatory=$true)][string]$unity_path, - [Parameter(Mandatory=$false)][string]$input_asset, - [Parameter(Mandatory=$false)][string]$package_name="Ros2ForUnity", - [Parameter(Mandatory=$false)][string]$output_dir -) - -$scriptPath = split-path -parent $MyInvocation.MyCommand.Definition -$temp_dir = $Env:TEMP - -if(-Not $PSBoundParameters.ContainsKey('input_asset')) { - $input_asset= Join-Path -Path $scriptPath -ChildPath "\install\asset\Ros2ForUnity" -} - -if(-Not $PSBoundParameters.ContainsKey('output_dir')) { - $output_dir= Join-Path -Path $scriptPath -ChildPath "\install\unity_package" -} - -if(-Not (Test-Path -Path "$input_asset")) { - Write-Host "Input asset '$input_asset' doesn't exist! Use 'build.ps1' to build project first." -ForegroundColor Red - exit 1 -} - -if(-Not (Test-Path -Path "$output_dir")) { - mkdir ${output_dir} | Out-Null -} - -& "$unity_path" -version | Tee-Object -Variable unity_version | Out-Null - -if ($unity_version -match '^[0-9]{4}\.[0-9]*\.[0-9]*[f]?[0-9]*$') { - Write-Host "Unity editor confirmed." -} else { - while (1) { - $confirmation = Read-Host "Can't confirm Unity editor. Do you want to force $unity_path as an Unity editor executable? [y]es or [n]o" - if ($confirmation -eq 'y' -or $confirmation -eq 'Y') { - break; - } elseif ( $confirmation -eq 'n' -or $confirmation -eq 'N' ) { - exit 1; - } else { - Write-Host "Please answer [y]es or [n]o."; - } - } -} -Write-Host "Using ${unity_path} editor." - -$tmp_project_path = Join-Path -Path "$temp_dir" -ChildPath "\ros2cs_unity_project\$unity_version" - -# Create temp project -if(Test-Path -Path "$tmp_project_path") { - Write-Host "Found existing temporary project for Unity $unity_version." - Remove-Item -Path "$tmp_project_path\Assets\*" -Force -Recurse -ErrorAction Ignore -} else { - Write-Host "Creating Unity temporary project for Unity $unity_version..." - & "$unity_path" -createProject "$tmp_project_path" -batchmode -quit | Out-Null -} - -# Copy asset -Write-Host "Copying asset '$input_asset' to export..." -Copy-Item -Path "$input_asset" -Destination "$tmp_project_path\Assets\$package_name" -Recurse - -# Creating asset -Write-Host "Saving unitypackage '$output_dir\$package_name.unitypackage'..." -& "$unity_path" -projectPath "$tmp_project_path" -exportPackage "Assets\$package_name" "$output_dir\$package_name.unitypackage" -batchmode -quit | Out-Null - -# Cleaning up -Write-Host "Cleaning up temporary project..." -Remove-Item -Path "$tmp_project_path\Assets\*" -Force -Recurse -ErrorAction Ignore - -Write-Host "Done!" -ForegroundColor Green - diff --git a/create_unity_package.sh b/create_unity_package.sh deleted file mode 100755 index 31635e0..0000000 --- a/create_unity_package.sh +++ /dev/null @@ -1,114 +0,0 @@ -#!/bin/bash - -SCRIPT=$(readlink -f $0) -SCRIPTPATH=`dirname $SCRIPT` - -display_usage() { - echo "This script creates a temporary Unity project in '/tmp' directory, copy input asset and makes an unity package out of it. Valid Unity license is required." - echo "" - echo "Usage:" - echo "create_unity_package.sh -u -i [INPUT_ASSET] -p [PACKAGE_NAME] -o [OUTPUT_DIR]" - echo "" - echo "UNITY_PATH - Unity editor executable path" - echo "INPUT_ASSET - input asset to pack into unity package, default = 'install/asset/Ros2ForUnity'" - echo "PACKAGE_NAME - unity package name, default = 'Ros2ForUnity'" - echo "OUTPUT_DIR - output file directory, default = 'install/unity_package'" -} - -UNITY_PATH="" -INPUT_ASSET="install/asset/Ros2ForUnity" -PACKAGE_NAME="Ros2ForUnity" -OUTPUT_DIR="$SCRIPTPATH/install/unity_package" - -while [[ $# -gt 0 ]]; do - key="$1" - - case $key in - -u|--unity-path) - UNITY_PATH="$2" - shift # past argument - shift # past value - ;; - -p|--package_name) - PACKAGE_NAME="$2" - shift # past argument - shift # past value - ;; - -i|--input-directory) - INPUT_ASSET="$2" - shift # past argument - shift # past value - ;; - -o|--output-directory) - OUTPUT_DIR="$2" - shift # past argument - shift # past value - ;; - -h|--help) - display_usage - exit 0 - shift # past argument - ;; - *) # unknown option - shift # past argument - ;; - esac -done - -if [ -z "$UNITY_PATH" ] || [ -z "$PACKAGE_NAME" ] || [ -z "$INPUT_ASSET" ] || [ -z "$OUTPUT_DIR" ]; then - echo -e "\nMissing arguments!" - echo "" - display_usage - exit 1 -fi - -if [ ! -d "$INPUT_ASSET" ]; then - echo "Input asset '$INPUT_ASSET' doesn't exist! Use 'build.sh' to build project first." - exit 1 -fi - -UNITY_VERSION=`$UNITY_PATH -version` - -# Test if unity editor is valid -if [[ $UNITY_VERSION =~ ^[0-9]{4}\.[0-9]*\.[0-9]*[f]?[0-9]*$ ]]; then - echo "Unity editor confirmed." -else - while true; do - read -p "Can't confirm Unity editor. Do you want to force \"$UNITY_PATH\" as an Unity editor executable? [y]es or [N]o: " yn - yn=${yn:-"n"} - case $yn in - [Yy]* ) break;; - [Nn]* ) exit 1;; - * ) echo "Please answer [y]es or [n]o.";; - esac - done -fi - -echo "Using \"${UNITY_PATH}\" editor." - -TMP_PROJECT_PATH=/tmp/ros2cs_unity_project/$UNITY_VERSION -# Create temp project -if [ -d "$TMP_PROJECT_PATH" ]; then - echo "Found existing temporary project for Unity $UNITY_VERSION." - rm -rf $TMP_PROJECT_PATH/Assets/* -else - rm -rf $TMP_PROJECT_PATH - echo "Creating Unity temporary project for Unity $UNITY_VERSION..." - $UNITY_PATH -createProject $TMP_PROJECT_PATH -batchmode -quit -fi - -# Copy asset -echo "Copying asset to export..." -cp -r "$INPUT_ASSET" "$TMP_PROJECT_PATH/Assets/$PACKAGE_NAME" - -# Creating asset -echo "Saving unitypackage '$OUTPUT_DIR/$PACKAGE_NAME.unitypackage'..." -mkdir -p $OUTPUT_DIR -$UNITY_PATH -projectPath "$TMP_PROJECT_PATH" -exportPackage "Assets/$PACKAGE_NAME" "$OUTPUT_DIR/$PACKAGE_NAME.unitypackage" -batchmode -quit - -# Cleaning up -echo "Cleaning up temporary project..." -rm -rf $TMP_PROJECT_PATH/Assets/* - -echo "Done!" - diff --git a/deploy_unity_plugins.sh b/deploy_unity_plugins.sh index cb787a6..1857ee4 100755 --- a/deploy_unity_plugins.sh +++ b/deploy_unity_plugins.sh @@ -1,20 +1,20 @@ -#!/bin/bash +#!/bin/sh -e -SCRIPT=$(readlink -f $0) -SCRIPTPATH=`dirname $SCRIPT` +SCRIPT=$(readlink -f "$0") +SCRIPTPATH=$(dirname "$SCRIPT") -if [ $# -eq 0 ] || [ $1 = "-h" ] || [ $1 = "--help" ]; then - echo "Usage:" +if [ $# -eq 0 ] || [ "$1" = "-h" ] || [ "$1" = "--help" ]; then + echo "Usage:" echo "deploy_unity_plugins.sh " echo "" echo "PLUGINS_DIR - Ros2ForUnity/Plugins folder." exit 1 fi -pluginDir=$1 +pluginDir="$1" -mkdir -p ${pluginDir}/Linux/x86_64/ -find install/lib/dotnet/ -maxdepth 1 -not -name "*.pdb" -type f -exec cp {} ${pluginDir} \; -cp $SCRIPTPATH/install/standalone/* ${pluginDir}/Linux/x86_64/ 2>/dev/null -find install/lib/ -maxdepth 1 -not -name "*_python.so" -type f -exec cp {} ${pluginDir}/Linux/x86_64/ \; -cp $SCRIPTPATH/install/resources/*.so ${pluginDir}/Linux/x86_64/ 2>/dev/null +mkdir -p "${pluginDir}/Linux/x86_64/" +find install/lib/dotnet/ -maxdepth 1 -not -name "*.pdb" -type f -exec cp {} "${pluginDir}" \; +cp "${SCRIPTPATH}/install/standalone/"* "${pluginDir}/Linux/x86_64/" 2> /dev/null +find install/lib/ -maxdepth 1 -not -name "*_python.so" -type f -exec cp {} "${pluginDir}/Linux/x86_64/" \; +cp "$SCRIPTPATH/install/resources/"*.so "${pluginDir}/Linux/x86_64/" 2> /dev/null diff --git a/src/Ros2ForUnity/COLCON_IGNORE.meta b/src/Ros2ForUnity/COLCON_IGNORE.meta new file mode 100644 index 0000000..7b36132 --- /dev/null +++ b/src/Ros2ForUnity/COLCON_IGNORE.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: da19dbd789dc969418bc2bab4ffe5509 +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/README.md b/src/Ros2ForUnity/README.md new file mode 100644 index 0000000..934225d --- /dev/null +++ b/src/Ros2ForUnity/README.md @@ -0,0 +1,5 @@ +# Ros2ForUnity + +This package provides bindings for interacting with ROS 2. + +For more information visit our [repository](https://github.com/RobotecAI/ros2-for-unity). \ No newline at end of file diff --git a/src/Ros2ForUnity/README.md.meta b/src/Ros2ForUnity/README.md.meta new file mode 100644 index 0000000..0660f46 --- /dev/null +++ b/src/Ros2ForUnity/README.md.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 10d65a7515439864f9d65cb62fd0c661 +TextScriptImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Resources.meta b/src/Ros2ForUnity/Resources.meta new file mode 100644 index 0000000..e75f4ba --- /dev/null +++ b/src/Ros2ForUnity/Resources.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 894a6d3a0b471a84f9d0aa13a40b6ddb +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Resources/.gitkeep b/src/Ros2ForUnity/Resources/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/Ros2ForUnity/Robotec.ai.Ros2ForUnity.asmdef b/src/Ros2ForUnity/Robotec.ai.Ros2ForUnity.asmdef new file mode 100644 index 0000000..f07e519 --- /dev/null +++ b/src/Ros2ForUnity/Robotec.ai.Ros2ForUnity.asmdef @@ -0,0 +1,14 @@ +{ + "name": "Robotec.ai.Ros2ForUnity", + "rootNamespace": "", + "references": [], + "includePlatforms": [], + "excludePlatforms": [], + "allowUnsafeCode": false, + "overrideReferences": false, + "precompiledReferences": [], + "autoReferenced": true, + "defineConstraints": [], + "versionDefines": [], + "noEngineReferences": false +} \ No newline at end of file diff --git a/src/Ros2ForUnity/Robotec.ai.Ros2ForUnity.asmdef.meta b/src/Ros2ForUnity/Robotec.ai.Ros2ForUnity.asmdef.meta new file mode 100644 index 0000000..d6042c1 --- /dev/null +++ b/src/Ros2ForUnity/Robotec.ai.Ros2ForUnity.asmdef.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 84f4b2120409cf84db84bb9514189cca +AssemblyDefinitionImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Runtime.meta b/src/Ros2ForUnity/Runtime.meta new file mode 100644 index 0000000..78830d4 --- /dev/null +++ b/src/Ros2ForUnity/Runtime.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 8c7f1682eb4beb741bc57a6970424ffc +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Runtime/PostInstall.cs b/src/Ros2ForUnity/Runtime/PostInstall.cs new file mode 100644 index 0000000..5c61eef --- /dev/null +++ b/src/Ros2ForUnity/Runtime/PostInstall.cs @@ -0,0 +1,49 @@ +// Copyright 2019-2022 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#if UNITY_EDITOR +using System.IO; +using System.Linq; +using System.Text.RegularExpressions; +using UnityEditor; +using UnityEditor.Build; +using UnityEditor.Build.Reporting; + +namespace ROS2 +{ + /// + /// An internal class responsible for installing versioned shared objects + /// + internal class PostInstall : IPostprocessBuildWithReport + { + public int callbackOrder { get => 0; } + + public void OnPostprocessBuild(BuildReport report) + { + if (report.summary.platform == BuildTarget.StandaloneLinux64) + { + // Copy versioned libraries (Unity skips them) + Regex soWithVersionReg = new Regex(@".*\.so(\.[0-9])+$"); + var versionedLibs = Directory.GetFiles(Setup.PluginPath).Where(path => soWithVersionReg.IsMatch(path)); + var outputDir = Directory.GetParent(report.summary.outputPath); + string execFilename = Path.GetFileNameWithoutExtension(report.summary.outputPath); + foreach (string libPath in versionedLibs) + { + FileUtil.CopyFileOrDirectory(libPath, $"{outputDir}/{execFilename}_Data/Plugins/{Path.GetFileName(libPath)}"); + } + } + } + } +} +#endif diff --git a/src/Ros2ForUnity/Scripts/ROS2ListenerExample.cs.meta b/src/Ros2ForUnity/Runtime/PostInstall.cs.meta similarity index 83% rename from src/Ros2ForUnity/Scripts/ROS2ListenerExample.cs.meta rename to src/Ros2ForUnity/Runtime/PostInstall.cs.meta index cbb05cf..20bc1a5 100644 --- a/src/Ros2ForUnity/Scripts/ROS2ListenerExample.cs.meta +++ b/src/Ros2ForUnity/Runtime/PostInstall.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 75a1bd43b302c4c578a744060319517e +guid: 0767da729d094694e80851516b08f92f MonoImporter: externalObjects: {} serializedVersion: 2 diff --git a/src/Ros2ForUnity/Runtime/ROS2ForUnity.cs b/src/Ros2ForUnity/Runtime/ROS2ForUnity.cs new file mode 100644 index 0000000..57f4772 --- /dev/null +++ b/src/Ros2ForUnity/Runtime/ROS2ForUnity.cs @@ -0,0 +1,91 @@ +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using System; +using UnityEngine; +using UnityEditor; + +namespace ROS2 +{ + +/// +/// An internal class responsible for handling checking, proper initialization and shutdown of ROS2cs, +/// +internal class ROS2ForUnity +{ + private static bool isInitialized = false; + + private void RegisterCtrlCHandler() + { +#if ENABLE_MONO + // Il2CPP build does not support Console.CancelKeyPress currently + Console.CancelKeyPress += (sender, eventArgs) => { + eventArgs.Cancel = true; + DestroyROS2ForUnity(); + }; +#endif + } + + internal ROS2ForUnity() + { + Ros2cs.Init(); + RegisterCtrlCHandler(); + +#if UNITY_EDITOR + EditorApplication.playModeStateChanged += this.EditorPlayStateChanged; + EditorApplication.quitting += this.DestroyROS2ForUnity; +#endif + isInitialized = true; + } + + /// + /// Check if ROS2 module is properly initialized and no shutdown was called yet + /// + /// The state of ROS2 module. Should be checked before attempting to create or use pubs/subs + public bool Ok() + { + if (!isInitialized) + { + return false; + } + return Ros2cs.Ok(); + } + + internal void DestroyROS2ForUnity() + { + if (isInitialized) + { + Debug.Log("Shutting down Ros2 For Unity"); + Ros2cs.Shutdown(); + isInitialized = false; + } + } + + ~ROS2ForUnity() + { + DestroyROS2ForUnity(); + } + +#if UNITY_EDITOR + void EditorPlayStateChanged(PlayModeStateChange change) + { + if (change == PlayModeStateChange.ExitingPlayMode) + { + DestroyROS2ForUnity(); + } + } +#endif +} + +} // namespace ROS2 diff --git a/src/Ros2ForUnity/Scripts/ROS2ForUnity.cs.meta b/src/Ros2ForUnity/Runtime/ROS2ForUnity.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/ROS2ForUnity.cs.meta rename to src/Ros2ForUnity/Runtime/ROS2ForUnity.cs.meta diff --git a/src/Ros2ForUnity/Scripts/ROS2Node.cs b/src/Ros2ForUnity/Runtime/ROS2Node.cs similarity index 97% rename from src/Ros2ForUnity/Scripts/ROS2Node.cs rename to src/Ros2ForUnity/Runtime/ROS2Node.cs index ad74fca..e007a0e 100644 --- a/src/Ros2ForUnity/Scripts/ROS2Node.cs +++ b/src/Ros2ForUnity/Runtime/ROS2Node.cs @@ -1,151 +1,151 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using System; -using System.Collections.Generic; -using UnityEngine; -using UnityEditor; - -namespace ROS2 -{ - -/// -/// A class representing a ros2 node. Multiple nodes can be used. Node can be removed by GC when not used anymore, -/// but will also be removed properly with Ros2cs Shutdown, which ROS2 for Unity performs on application quit -/// The node should be constructed through ROS2UnityComponent class, which also handles spinning -/// -public class ROS2Node -{ - internal INode node; - public ROS2Clock clock; - public string name; - - // Use ROS2UnityComponent to create a node - internal ROS2Node(string unityROS2NodeName = "unity_ros2_node") - { - name = unityROS2NodeName; - node = Ros2cs.CreateNode(name); - clock = new ROS2Clock(); - } - - ~ROS2Node() - { - Ros2cs.RemoveNode(node); - } - - private static void ThrowIfUninitialized(string callContext) - { - if (!Ros2cs.Ok()) - { - throw new InvalidOperationException("Ros2 For Unity is not initialized, can't " + callContext); - } - } - - /// - /// Create a publisher with QoS suitable for sensor data - /// - /// The publisher - /// topic that will be used for publishing - public Publisher CreateSensorPublisher(string topicName) where T : Message, new() - { - QualityOfServiceProfile sensorProfile = new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA); - return CreatePublisher(topicName, sensorProfile); - } - - /// - /// Create a publisher with indicated QoS. - /// - /// The publisher - /// topic that will be used for publishing - /// QoS for publishing. If no QoS is selected, it will default to reliable, keep 10 last - public Publisher CreatePublisher(string topicName, QualityOfServiceProfile qos = null) where T : Message, new() - { - ThrowIfUninitialized("create publisher"); - return node.CreatePublisher(topicName, qos); - } - - /// - /// Create a subscription - /// - /// The subscription - /// topic to subscribe to - /// QoS for subscription. If no QoS is selected, it will default to reliable, keep 10 last - public Subscription CreateSubscription(string topicName, Action callback, - QualityOfServiceProfile qos = null) where T : Message, new() - { - if (qos == null) - { - qos = new QualityOfServiceProfile(QosPresetProfile.DEFAULT); - } - ThrowIfUninitialized("create subscription"); - return node.CreateSubscription(topicName, callback, qos); - } - - - /// - /// Remove existing subscription (returned earlier with CreateSubscription) - /// - /// The whether subscription was found (e. g. false if removed earlier elsewhere) - /// subscrition to remove, returned from CreateSubscription - public bool RemoveSubscription(ISubscriptionBase subscription) - { - ThrowIfUninitialized("remove subscription"); - return node.RemoveSubscription(subscription); - } - - /// - /// Remove existing publisher - /// - /// The whether publisher was found (e. g. false if removed earlier elsewhere) - /// publisher to remove, returned from CreatePublisher or CreateSensorPublisher - public bool RemovePublisher(IPublisherBase publisher) - { - ThrowIfUninitialized("remove publisher"); - return node.RemovePublisher(publisher); - } - - /// - public Service CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) - where I : Message, new() - where O : Message, new() - { - ThrowIfUninitialized("create service"); - return node.CreateService(topic, callback, qos); - } - - /// - public bool RemoveService(IServiceBase service) - { - ThrowIfUninitialized("remove service"); - return node.RemoveService(service); - } - - /// - public Client CreateClient(string topic, QualityOfServiceProfile qos = null) - where I : Message, new() - where O : Message, new() - { - ThrowIfUninitialized(callContext: "create client"); - return node.CreateClient(topic, qos); - } - - /// - public bool RemoveClient(IClientBase client) - { - ThrowIfUninitialized(callContext: "remove client"); - return node.RemoveClient(client); - } -} - -} // namespace ROS2 +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using System; +using System.Collections.Generic; +using UnityEngine; +using UnityEditor; + +namespace ROS2 +{ + +/// +/// A class representing a ros2 node. Multiple nodes can be used. Node can be removed by GC when not used anymore, +/// but will also be removed properly with Ros2cs Shutdown, which ROS2 for Unity performs on application quit +/// The node should be constructed through ROS2UnityComponent class, which also handles spinning +/// +public class ROS2Node +{ + internal INode node; + public ROS2Clock clock; + public string name; + + // Use ROS2UnityComponent to create a node + internal ROS2Node(string unityROS2NodeName = "unity_ros2_node") + { + name = unityROS2NodeName; + node = Ros2cs.CreateNode(name); + clock = new ROS2Clock(); + } + + ~ROS2Node() + { + Ros2cs.RemoveNode(node); + } + + private static void ThrowIfUninitialized(string callContext) + { + if (!Ros2cs.Ok()) + { + throw new InvalidOperationException("Ros2 For Unity is not initialized, can't " + callContext); + } + } + + /// + /// Create a publisher with QoS suitable for sensor data + /// + /// The publisher + /// topic that will be used for publishing + public Publisher CreateSensorPublisher(string topicName) where T : Message, new() + { + QualityOfServiceProfile sensorProfile = new QualityOfServiceProfile(QosPresetProfile.SENSOR_DATA); + return CreatePublisher(topicName, sensorProfile); + } + + /// + /// Create a publisher with indicated QoS. + /// + /// The publisher + /// topic that will be used for publishing + /// QoS for publishing. If no QoS is selected, it will default to reliable, keep 10 last + public Publisher CreatePublisher(string topicName, QualityOfServiceProfile qos = null) where T : Message, new() + { + ThrowIfUninitialized("create publisher"); + return node.CreatePublisher(topicName, qos); + } + + /// + /// Create a subscription + /// + /// The subscription + /// topic to subscribe to + /// QoS for subscription. If no QoS is selected, it will default to reliable, keep 10 last + public Subscription CreateSubscription(string topicName, Action callback, + QualityOfServiceProfile qos = null) where T : Message, new() + { + if (qos == null) + { + qos = new QualityOfServiceProfile(QosPresetProfile.DEFAULT); + } + ThrowIfUninitialized("create subscription"); + return node.CreateSubscription(topicName, callback, qos); + } + + + /// + /// Remove existing subscription (returned earlier with CreateSubscription) + /// + /// The whether subscription was found (e. g. false if removed earlier elsewhere) + /// subscrition to remove, returned from CreateSubscription + public bool RemoveSubscription(ISubscriptionBase subscription) + { + ThrowIfUninitialized("remove subscription"); + return node.RemoveSubscription(subscription); + } + + /// + /// Remove existing publisher + /// + /// The whether publisher was found (e. g. false if removed earlier elsewhere) + /// publisher to remove, returned from CreatePublisher or CreateSensorPublisher + public bool RemovePublisher(IPublisherBase publisher) + { + ThrowIfUninitialized("remove publisher"); + return node.RemovePublisher(publisher); + } + + /// + public Service CreateService(string topic, Func callback, QualityOfServiceProfile qos = null) + where I : Message, new() + where O : Message, new() + { + ThrowIfUninitialized("create service"); + return node.CreateService(topic, callback, qos); + } + + /// + public bool RemoveService(IServiceBase service) + { + ThrowIfUninitialized("remove service"); + return node.RemoveService(service); + } + + /// + public Client CreateClient(string topic, QualityOfServiceProfile qos = null) + where I : Message, new() + where O : Message, new() + { + ThrowIfUninitialized(callContext: "create client"); + return node.CreateClient(topic, qos); + } + + /// + public bool RemoveClient(IClientBase client) + { + ThrowIfUninitialized(callContext: "remove client"); + return node.RemoveClient(client); + } +} + +} // namespace ROS2 diff --git a/src/Ros2ForUnity/Scripts/ROS2Node.cs.meta b/src/Ros2ForUnity/Runtime/ROS2Node.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/ROS2Node.cs.meta rename to src/Ros2ForUnity/Runtime/ROS2Node.cs.meta diff --git a/src/Ros2ForUnity/Scripts/ROS2PerformanceTest.cs b/src/Ros2ForUnity/Runtime/ROS2PerformanceTest.cs similarity index 96% rename from src/Ros2ForUnity/Scripts/ROS2PerformanceTest.cs rename to src/Ros2ForUnity/Runtime/ROS2PerformanceTest.cs index 3b70f3b..84b7e74 100644 --- a/src/Ros2ForUnity/Scripts/ROS2PerformanceTest.cs +++ b/src/Ros2ForUnity/Runtime/ROS2PerformanceTest.cs @@ -1,137 +1,137 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using UnityEngine; -using System.Threading; - -namespace ROS2 -{ - -/// -/// An example class provided for performance testing of ROS2 communication -/// -public class ROS2PerformanceTest : MonoBehaviour -{ - public int messageSize = 10000; - public int rate = 10; - private int interval_ms = 100; - private ROS2UnityComponent ros2Unity; - private ROS2Node ros2Node; - private IPublisher perf_pub; - sensor_msgs.msg.PointCloud2 msg; - private bool initialized = false; - - void Start() - { - ros2Unity = GetComponent(); - PrepMessage(); - } - - void OnValidate() - { - if (rate < 1) - { - interval_ms = 0; - } - else - { - interval_ms = 1000 / rate; - } - PrepMessage(); - } - - private void Publish() - { - while(true) - { - if (ros2Unity.Ok()) - { - if (ros2Node == null) - { - ros2Node = ros2Unity.CreateNode("ros2_unity_performance_test_node"); - perf_pub = ros2Node.CreateSensorPublisher("perf_chatter"); - } - - var msgWithHeader = msg as MessageWithHeader; - ros2Node.clock.UpdateROSTimestamp(ref msgWithHeader); - perf_pub.Publish(msg); - if (interval_ms > 0) - { - Thread.Sleep(interval_ms); - } - } - } - } - - void FixedUpdate() - { - if (!initialized) - { - Thread publishThread = new Thread(() => Publish()); - publishThread.Start(); - initialized = true; - } - } - - private void AssignField(ref sensor_msgs.msg.PointField pf, string n, uint off, byte dt, uint count) - { - pf.Name = n; - pf.Offset = off; - pf.Datatype = dt; - pf.Count = count; - } - - private void PrepMessage() - { - uint count = (uint)messageSize; //point per message - uint fieldsSize = 16; - uint rowSize = count * fieldsSize; - msg = new sensor_msgs.msg.PointCloud2() - { - Height = 1, - Width = count, - Is_bigendian = false, - Is_dense = true, - Point_step = fieldsSize, - Row_step = rowSize, - Data = new byte[rowSize * 1] - }; - uint pointFieldCount = 4; - msg.Fields = new sensor_msgs.msg.PointField[pointFieldCount]; - for (int i = 0; i < pointFieldCount; ++i) - { - msg.Fields[i] = new sensor_msgs.msg.PointField(); - } - - AssignField(ref msg.Fields[0], "x", 0, 7, 1); - AssignField(ref msg.Fields[1], "y", 4, 7, 1); - AssignField(ref msg.Fields[2], "z", 8, 7, 1); - AssignField(ref msg.Fields[3], "intensity", 12, 7, 1); - float[] pointsArray = new float[count * msg.Fields.Length]; - - var floatIndex = 0; - for (int i = 0; i < count; ++i) - { - float intensity = 100; - pointsArray[floatIndex++] = 1; - pointsArray[floatIndex++] = 2; - pointsArray[floatIndex++] = 3; - pointsArray[floatIndex++] = intensity; - } - System.Buffer.BlockCopy(pointsArray, 0, msg.Data, 0, msg.Data.Length); - msg.SetHeaderFrame("pc"); - } -} - -} // namespace ROS2 +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using UnityEngine; +using System.Threading; + +namespace ROS2 +{ + +/// +/// An example class provided for performance testing of ROS2 communication +/// +public class ROS2PerformanceTest : MonoBehaviour +{ + public int messageSize = 10000; + public int rate = 10; + private int interval_ms = 100; + private ROS2UnityComponent ros2Unity; + private ROS2Node ros2Node; + private IPublisher perf_pub; + sensor_msgs.msg.PointCloud2 msg; + private bool initialized = false; + + void Start() + { + ros2Unity = GetComponent(); + PrepMessage(); + } + + void OnValidate() + { + if (rate < 1) + { + interval_ms = 0; + } + else + { + interval_ms = 1000 / rate; + } + PrepMessage(); + } + + private void Publish() + { + while(true) + { + if (ros2Unity.Ok()) + { + if (ros2Node == null) + { + ros2Node = ros2Unity.CreateNode("ros2_unity_performance_test_node"); + perf_pub = ros2Node.CreateSensorPublisher("perf_chatter"); + } + + var msgWithHeader = msg as MessageWithHeader; + ros2Node.clock.UpdateROSTimestamp(ref msgWithHeader); + perf_pub.Publish(msg); + if (interval_ms > 0) + { + Thread.Sleep(interval_ms); + } + } + } + } + + void FixedUpdate() + { + if (!initialized) + { + Thread publishThread = new Thread(() => Publish()); + publishThread.Start(); + initialized = true; + } + } + + private void AssignField(ref sensor_msgs.msg.PointField pf, string n, uint off, byte dt, uint count) + { + pf.Name = n; + pf.Offset = off; + pf.Datatype = dt; + pf.Count = count; + } + + private void PrepMessage() + { + uint count = (uint)messageSize; //point per message + uint fieldsSize = 16; + uint rowSize = count * fieldsSize; + msg = new sensor_msgs.msg.PointCloud2() + { + Height = 1, + Width = count, + Is_bigendian = false, + Is_dense = true, + Point_step = fieldsSize, + Row_step = rowSize, + Data = new byte[rowSize * 1] + }; + uint pointFieldCount = 4; + msg.Fields = new sensor_msgs.msg.PointField[pointFieldCount]; + for (int i = 0; i < pointFieldCount; ++i) + { + msg.Fields[i] = new sensor_msgs.msg.PointField(); + } + + AssignField(ref msg.Fields[0], "x", 0, 7, 1); + AssignField(ref msg.Fields[1], "y", 4, 7, 1); + AssignField(ref msg.Fields[2], "z", 8, 7, 1); + AssignField(ref msg.Fields[3], "intensity", 12, 7, 1); + float[] pointsArray = new float[count * msg.Fields.Length]; + + var floatIndex = 0; + for (int i = 0; i < count; ++i) + { + float intensity = 100; + pointsArray[floatIndex++] = 1; + pointsArray[floatIndex++] = 2; + pointsArray[floatIndex++] = 3; + pointsArray[floatIndex++] = intensity; + } + System.Buffer.BlockCopy(pointsArray, 0, msg.Data, 0, msg.Data.Length); + msg.SetHeaderFrame("pc"); + } +} + +} // namespace ROS2 diff --git a/src/Ros2ForUnity/Scripts/ROS2PerformanceTest.cs.meta b/src/Ros2ForUnity/Runtime/ROS2PerformanceTest.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/ROS2PerformanceTest.cs.meta rename to src/Ros2ForUnity/Runtime/ROS2PerformanceTest.cs.meta diff --git a/src/Ros2ForUnity/Scripts/ROS2UnityComponent.cs b/src/Ros2ForUnity/Runtime/ROS2UnityComponent.cs similarity index 96% rename from src/Ros2ForUnity/Scripts/ROS2UnityComponent.cs rename to src/Ros2ForUnity/Runtime/ROS2UnityComponent.cs index 24018c8..7c916e2 100644 --- a/src/Ros2ForUnity/Scripts/ROS2UnityComponent.cs +++ b/src/Ros2ForUnity/Runtime/ROS2UnityComponent.cs @@ -1,164 +1,164 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using UnityEngine; -using System; -using System.Collections.Generic; -using System.Threading; -using ROS2; - -namespace ROS2 -{ - -/// -/// The principal MonoBehaviour class for handling ros2 nodes and executables. -/// Use this to create ros2 node, check ros2 status. -/// Spins and executes actions (e. g. clock, sensor publish triggers) in a dedicated thread -/// TODO: this is meant to be used as a one-of (a singleton). Enforce. However, things should work -/// anyway with more than one since the underlying library can handle multiple init and shutdown calls, -/// and does node name uniqueness check independently. -/// -public class ROS2UnityComponent : MonoBehaviour -{ - private ROS2ForUnity ros2forUnity; - private List nodes; - private List ros2csNodes; // For performance in spinning - private List executableActions; - private bool initialized = false; - private bool quitting = false; - private int interval = 2; // Spinning / executor interval in ms - private object mutex = new object(); - private double spinTimeout = 0.0001; - - public bool Ok() - { - lock (mutex) - { - if (ros2forUnity == null) - LazyConstruct(); - return (nodes != null && ros2forUnity.Ok()); - } - } - - private void LazyConstruct() - { - lock (mutex) - { - if (ros2forUnity != null) - return; - - ros2forUnity = new ROS2ForUnity(); - nodes = new List(); - ros2csNodes = new List(); - executableActions = new List(); - } - } - - void Start() - { - LazyConstruct(); - } - - public ROS2Node CreateNode(string name) - { - LazyConstruct(); - - lock (mutex) - { - foreach (ROS2Node n in nodes) - { // Assumed to be a rare operation on rather small (<1k) list - if (n.name == name) - { - throw new InvalidOperationException("Cannot create node " + name + ". A node with this name already exists!"); - } - } - ROS2Node node = new ROS2Node(name); - nodes.Add(node); - ros2csNodes.Add(node.node); - return node; - } - } - - public void RemoveNode(ROS2Node node) - { - lock (mutex) - { - ros2csNodes.Remove(node.node); - nodes.Remove(node); //Node will be later deleted if unused, by GC - } - } - - /// - /// Works as a simple executor registration analogue. These functions will be called with each Tick() - /// Actions need to take care of correct call resolution by checking in their body (TODO) - /// Make sure actions are lightweight (TODO - separate out threads for spinning and executables?) - /// - public void RegisterExecutable(Action executable) - { - LazyConstruct(); - - lock (mutex) - { - executableActions.Add(executable); - } - } - - public void UnregisterExecutable(Action executable) - { - lock (mutex) - { - executableActions.Remove(executable); - } - } - - /// - /// "Executor" thread will tick all clocks and spin the node - /// - private void Tick() - { - while (!quitting) - { - if (Ok()) - { - lock (mutex) - { - foreach (Action action in executableActions) - { - action(); - } - Ros2cs.SpinOnce(ros2csNodes, spinTimeout); - } - } - Thread.Sleep(interval); - } - } - - void FixedUpdate() - { - if (!initialized) - { - Thread publishThread = new Thread(() => Tick()); - publishThread.Start(); - initialized = true; - } - } - - void OnApplicationQuit() - { - quitting = true; - ros2forUnity.DestroyROS2ForUnity(); - } -} - -} // namespace ROS2 +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using UnityEngine; +using System; +using System.Collections.Generic; +using System.Threading; +using ROS2; + +namespace ROS2 +{ + +/// +/// The principal MonoBehaviour class for handling ros2 nodes and executables. +/// Use this to create ros2 node, check ros2 status. +/// Spins and executes actions (e. g. clock, sensor publish triggers) in a dedicated thread +/// TODO: this is meant to be used as a one-of (a singleton). Enforce. However, things should work +/// anyway with more than one since the underlying library can handle multiple init and shutdown calls, +/// and does node name uniqueness check independently. +/// +public class ROS2UnityComponent : MonoBehaviour +{ + private ROS2ForUnity ros2forUnity; + private List nodes; + private List ros2csNodes; // For performance in spinning + private List executableActions; + private bool initialized = false; + private bool quitting = false; + private int interval = 2; // Spinning / executor interval in ms + private object mutex = new object(); + private double spinTimeout = 0.0001; + + public bool Ok() + { + lock (mutex) + { + if (ros2forUnity == null) + LazyConstruct(); + return (nodes != null && ros2forUnity.Ok()); + } + } + + private void LazyConstruct() + { + lock (mutex) + { + if (ros2forUnity != null) + return; + + ros2forUnity = new ROS2ForUnity(); + nodes = new List(); + ros2csNodes = new List(); + executableActions = new List(); + } + } + + void Start() + { + LazyConstruct(); + } + + public ROS2Node CreateNode(string name) + { + LazyConstruct(); + + lock (mutex) + { + foreach (ROS2Node n in nodes) + { // Assumed to be a rare operation on rather small (<1k) list + if (n.name == name) + { + throw new InvalidOperationException("Cannot create node " + name + ". A node with this name already exists!"); + } + } + ROS2Node node = new ROS2Node(name); + nodes.Add(node); + ros2csNodes.Add(node.node); + return node; + } + } + + public void RemoveNode(ROS2Node node) + { + lock (mutex) + { + ros2csNodes.Remove(node.node); + nodes.Remove(node); //Node will be later deleted if unused, by GC + } + } + + /// + /// Works as a simple executor registration analogue. These functions will be called with each Tick() + /// Actions need to take care of correct call resolution by checking in their body (TODO) + /// Make sure actions are lightweight (TODO - separate out threads for spinning and executables?) + /// + public void RegisterExecutable(Action executable) + { + LazyConstruct(); + + lock (mutex) + { + executableActions.Add(executable); + } + } + + public void UnregisterExecutable(Action executable) + { + lock (mutex) + { + executableActions.Remove(executable); + } + } + + /// + /// "Executor" thread will tick all clocks and spin the node + /// + private void Tick() + { + while (!quitting) + { + if (Ok()) + { + lock (mutex) + { + foreach (Action action in executableActions) + { + action(); + } + Ros2cs.SpinOnce(ros2csNodes, spinTimeout); + } + } + Thread.Sleep(interval); + } + } + + void FixedUpdate() + { + if (!initialized) + { + Thread publishThread = new Thread(() => Tick()); + publishThread.Start(); + initialized = true; + } + } + + void OnApplicationQuit() + { + quitting = true; + ros2forUnity.DestroyROS2ForUnity(); + } +} + +} // namespace ROS2 diff --git a/src/Ros2ForUnity/Scripts/ROS2UnityComponent.cs.meta b/src/Ros2ForUnity/Runtime/ROS2UnityComponent.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/ROS2UnityComponent.cs.meta rename to src/Ros2ForUnity/Runtime/ROS2UnityComponent.cs.meta diff --git a/src/Ros2ForUnity/Scripts/ROS2UnityCore.cs b/src/Ros2ForUnity/Runtime/ROS2UnityCore.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/ROS2UnityCore.cs rename to src/Ros2ForUnity/Runtime/ROS2UnityCore.cs diff --git a/src/Ros2ForUnity/Scripts/ROS2UnityCore.cs.meta b/src/Ros2ForUnity/Runtime/ROS2UnityCore.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/ROS2UnityCore.cs.meta rename to src/Ros2ForUnity/Runtime/ROS2UnityCore.cs.meta diff --git a/src/Ros2ForUnity/Scripts/Sensor.cs b/src/Ros2ForUnity/Runtime/Sensor.cs old mode 100755 new mode 100644 similarity index 100% rename from src/Ros2ForUnity/Scripts/Sensor.cs rename to src/Ros2ForUnity/Runtime/Sensor.cs diff --git a/src/Ros2ForUnity/Scripts/Sensor.cs.meta b/src/Ros2ForUnity/Runtime/Sensor.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/Sensor.cs.meta rename to src/Ros2ForUnity/Runtime/Sensor.cs.meta diff --git a/src/Ros2ForUnity/Runtime/Setup.cs b/src/Ros2ForUnity/Runtime/Setup.cs new file mode 100644 index 0000000..ef3453c --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Setup.cs @@ -0,0 +1,187 @@ +using System; +using System.IO; +using System.Xml; +using System.Xml.Serialization; +using UnityEditor; +using UnityEngine; +using UnityEngine.Scripting; + +namespace ROS2 +{ + /// + /// Class used for setup tasks. + /// + public static class Setup + { + private const string MetadataFileName = "ros2-for-unity"; + + [XmlRoot("ros2-for-unity", IsNullable = false)] + public sealed class MetaData + { + [XmlElement("ros")] + public string RosVersion; + + [XmlElement("standalone")] + public bool IsStandalone; + + [Preserve] + public MetaData() + { } + + public static MetaData Load() + { + XmlSerializer serializer = new XmlSerializer(typeof(MetaData)); + TextAsset asset = Resources.Load(MetadataFileName); + return (MetaData)serializer.Deserialize(new MemoryStream(asset.bytes)); + } + } + + private enum Platform + { + Windows, + Linux + } + + private static Platform CurrentPlatform + { + get + { + switch (Application.platform) + { + case RuntimePlatform.LinuxEditor: + case RuntimePlatform.LinuxPlayer: + return Platform.Linux; + case RuntimePlatform.WindowsEditor: + case RuntimePlatform.WindowsPlayer: + return Platform.Windows; + default: + throw new NotSupportedException("Only Linux and Windows are supported"); + } + } + } + + internal static string PluginPath + { + get + { + switch (Application.platform) + { + case RuntimePlatform.LinuxEditor: + return Path.GetFullPath("Packages/ai.robotec.ros2-for-unity/Plugins") + "/Linux/x86_64"; + case RuntimePlatform.LinuxPlayer: + return Path.GetFullPath(Application.dataPath) + "/Plugins"; + case RuntimePlatform.WindowsEditor: + return Path.GetFullPath("Packages/ai.robotec.ros2-for-unity/Plugins").Replace("/", "\\") + "\\Windows\\x86_64"; + case RuntimePlatform.WindowsPlayer: + return Path.GetFullPath(Application.dataPath).Replace("/", "\\") + "\\Plugins\\x86_64"; + default: + throw new NotSupportedException("Only Linux and Windows are supported"); + } + } + } + + private static string GetSourcedROS() + { + return Environment.GetEnvironmentVariable("ROS_DISTRO"); + } + + private static void TerminateApplication() + { +#if UNITY_EDITOR + EditorApplication.isPlaying = false; + throw new Exception("Failed to setup ROS2ForUnity"); +#else + Application.Quit(1); +#endif + } + + /// + /// Setup PATH for native libraries. + /// + /// + /// Is called automatically when Unity starts, has to be called manually when running in edit mode. + /// Note that on Linux, LD_LIBRARY_PATH as used for dlopen() is determined on process start and this change won't + /// affect it. Ros2 looks for rmw implementation based on this variable (independently) and the change + /// is effective for this process, however rmw implementation's dependencies itself are loaded by dynamic linker + /// anyway so setting it for Linux is pointless. + /// + [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.SubsystemRegistration)] + public static void SetupPath() + { + string pathVariable; + string pathSeparator; + switch (CurrentPlatform) + { + case Platform.Windows: + pathVariable = "PATH"; + pathSeparator = ";"; + break; + case Platform.Linux: + pathVariable = "LD_LIBRARY_PATH"; + pathSeparator = ":"; + ROS2.GlobalVariables.absolutePath = $"{PluginPath}/"; + break; + default: + throw new NotSupportedException("unsupported platform"); + } + Environment.SetEnvironmentVariable(pathVariable, $"{PluginPath}{pathSeparator}{Environment.GetEnvironmentVariable(pathVariable)}"); + } + + /// + /// Check for correct ROS2 setup. + /// + /// + /// Is called automatically when Unity starts, has to be called manually when running in edit mode. + /// + [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.BeforeSplashScreen)] + public static void CheckIntegrity() + { + MetaData metaData = null; + try + { + metaData = MetaData.Load(); + } + catch (Exception error) + { + Debug.LogException(error); + TerminateApplication(); + } + string sourced = GetSourcedROS(); + if (!metaData.IsStandalone && sourced != metaData.RosVersion) + { + Debug.LogError( + $"ROS2 version in 'ros2cs' metadata ({metaData.RosVersion}) doesn't match currently sourced version ({sourced}). " + + "This is caused by mixing versions/builds. Plugin might not work correctly." + ); + TerminateApplication(); + } + if (metaData.IsStandalone && !string.IsNullOrEmpty(sourced)) + { + Debug.LogError( + "You should not source ROS2 in 'ros2-for-unity' standalone build. " + + "Plugin might not work correctly." + ); + TerminateApplication(); + } + string rosVersion = string.IsNullOrEmpty(sourced) ? metaData.RosVersion : sourced; + string standalone = metaData.IsStandalone ? "standalone" : "non-standalone"; + Debug.Log($"ROS2 version: {rosVersion}. Build type: {standalone}. RMW: {Ros2cs.GetRMWImplementation()}"); + } + + /// + /// Connect to the Unity logging system. + /// + /// + /// Is called automatically when Unity starts, has to be called manually when running in edit mode. + /// + [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.BeforeSplashScreen)] + public static void ConnectLoggers() + { + Ros2csLogger.setCallback(LogLevel.ERROR, Debug.LogError); + Ros2csLogger.setCallback(LogLevel.WARNING, Debug.LogWarning); + Ros2csLogger.setCallback(LogLevel.INFO, Debug.Log); + Ros2csLogger.setCallback(LogLevel.DEBUG, Debug.Log); + Ros2csLogger.LogLevel = LogLevel.WARNING; + } + } +} diff --git a/src/Ros2ForUnity/Scripts/ROS2TalkerExample.cs.meta b/src/Ros2ForUnity/Runtime/Setup.cs.meta similarity index 83% rename from src/Ros2ForUnity/Scripts/ROS2TalkerExample.cs.meta rename to src/Ros2ForUnity/Runtime/Setup.cs.meta index 2b00d7f..e0f62df 100644 --- a/src/Ros2ForUnity/Scripts/ROS2TalkerExample.cs.meta +++ b/src/Ros2ForUnity/Runtime/Setup.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: 72620fb0a9290863f8643557405c48e3 +guid: 404afd5bfc22f194998ef4fd30e27421 MonoImporter: externalObjects: {} serializedVersion: 2 diff --git a/src/Ros2ForUnity/Runtime/Time.meta b/src/Ros2ForUnity/Runtime/Time.meta new file mode 100644 index 0000000..dcba2d3 --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Time.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 4efa40fb7b9808c41bd284557a13babe +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/Time/DotnetTimeSource.cs b/src/Ros2ForUnity/Runtime/Time/DotnetTimeSource.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/DotnetTimeSource.cs rename to src/Ros2ForUnity/Runtime/Time/DotnetTimeSource.cs diff --git a/src/Ros2ForUnity/Runtime/Time/DotnetTimeSource.cs.meta b/src/Ros2ForUnity/Runtime/Time/DotnetTimeSource.cs.meta new file mode 100644 index 0000000..564e48d --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Time/DotnetTimeSource.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7914aca812db2f44db3700cabff4403c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/Time/ITimeSource.cs b/src/Ros2ForUnity/Runtime/Time/ITimeSource.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/ITimeSource.cs rename to src/Ros2ForUnity/Runtime/Time/ITimeSource.cs diff --git a/src/Ros2ForUnity/Runtime/Time/ITimeSource.cs.meta b/src/Ros2ForUnity/Runtime/Time/ITimeSource.cs.meta new file mode 100644 index 0000000..fb7ab03 --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Time/ITimeSource.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 090a4ccd865ac7848bf7068b7b5a375d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs b/src/Ros2ForUnity/Runtime/Time/ROS2Clock.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs rename to src/Ros2ForUnity/Runtime/Time/ROS2Clock.cs diff --git a/src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs.meta b/src/Ros2ForUnity/Runtime/Time/ROS2Clock.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs.meta rename to src/Ros2ForUnity/Runtime/Time/ROS2Clock.cs.meta diff --git a/src/Ros2ForUnity/Scripts/Time/ROS2ScalableTimeSource.cs b/src/Ros2ForUnity/Runtime/Time/ROS2ScalableTimeSource.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/ROS2ScalableTimeSource.cs rename to src/Ros2ForUnity/Runtime/Time/ROS2ScalableTimeSource.cs diff --git a/src/Ros2ForUnity/Runtime/Time/ROS2ScalableTimeSource.cs.meta b/src/Ros2ForUnity/Runtime/Time/ROS2ScalableTimeSource.cs.meta new file mode 100644 index 0000000..62db977 --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Time/ROS2ScalableTimeSource.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cd4e62669a2f6e8448ec1ff3aadb7f9f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/Time/ROS2TimeSource.cs b/src/Ros2ForUnity/Runtime/Time/ROS2TimeSource.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/ROS2TimeSource.cs rename to src/Ros2ForUnity/Runtime/Time/ROS2TimeSource.cs diff --git a/src/Ros2ForUnity/Runtime/Time/ROS2TimeSource.cs.meta b/src/Ros2ForUnity/Runtime/Time/ROS2TimeSource.cs.meta new file mode 100644 index 0000000..1d48b78 --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Time/ROS2TimeSource.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 3257f65def4dbdb4ea590d96fc9ea2ed +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/Time/TimeUtils.cs b/src/Ros2ForUnity/Runtime/Time/TimeUtils.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/TimeUtils.cs rename to src/Ros2ForUnity/Runtime/Time/TimeUtils.cs diff --git a/src/Ros2ForUnity/Runtime/Time/TimeUtils.cs.meta b/src/Ros2ForUnity/Runtime/Time/TimeUtils.cs.meta new file mode 100644 index 0000000..1ce413a --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Time/TimeUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 656958edf824c4e48b0d30497efc1c93 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/Time/UnityTimeSource.cs b/src/Ros2ForUnity/Runtime/Time/UnityTimeSource.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Time/UnityTimeSource.cs rename to src/Ros2ForUnity/Runtime/Time/UnityTimeSource.cs diff --git a/src/Ros2ForUnity/Runtime/Time/UnityTimeSource.cs.meta b/src/Ros2ForUnity/Runtime/Time/UnityTimeSource.cs.meta new file mode 100644 index 0000000..40cf6ce --- /dev/null +++ b/src/Ros2ForUnity/Runtime/Time/UnityTimeSource.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7cb0d92575551a24da6687659f12b67a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/Transformations.cs b/src/Ros2ForUnity/Runtime/Transformations.cs similarity index 100% rename from src/Ros2ForUnity/Scripts/Transformations.cs rename to src/Ros2ForUnity/Runtime/Transformations.cs diff --git a/src/Ros2ForUnity/Scripts/Transformations.cs.meta b/src/Ros2ForUnity/Runtime/Transformations.cs.meta similarity index 100% rename from src/Ros2ForUnity/Scripts/Transformations.cs.meta rename to src/Ros2ForUnity/Runtime/Transformations.cs.meta diff --git a/src/Ros2ForUnity/Samples~/Clients and Services/Scenes.meta b/src/Ros2ForUnity/Samples~/Clients and Services/Scenes.meta new file mode 100644 index 0000000..83c741b --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scenes.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 6ea315d0fd7389c41b19996891e99ae3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Samples~/Clients and Services/Scenes/SampleScene.unity b/src/Ros2ForUnity/Samples~/Clients and Services/Scenes/SampleScene.unity new file mode 100644 index 0000000..fc2fa6e --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scenes/SampleScene.unity @@ -0,0 +1,373 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!29 &1 +OcclusionCullingSettings: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_OcclusionBakeSettings: + smallestOccluder: 5 + smallestHole: 0.25 + backfaceThreshold: 100 + m_SceneGUID: 00000000000000000000000000000000 + m_OcclusionCullingData: {fileID: 0} +--- !u!104 &2 +RenderSettings: + m_ObjectHideFlags: 0 + serializedVersion: 9 + m_Fog: 0 + m_FogColor: {r: 0.5, g: 0.5, b: 0.5, a: 1} + m_FogMode: 3 + m_FogDensity: 0.01 + m_LinearFogStart: 0 + m_LinearFogEnd: 300 + m_AmbientSkyColor: {r: 0.212, g: 0.227, b: 0.259, a: 1} + m_AmbientEquatorColor: {r: 0.114, g: 0.125, b: 0.133, a: 1} + m_AmbientGroundColor: {r: 0.047, g: 0.043, b: 0.035, a: 1} + m_AmbientIntensity: 1 + m_AmbientMode: 0 + m_SubtractiveShadowColor: {r: 0.42, g: 0.478, b: 0.627, a: 1} + m_SkyboxMaterial: {fileID: 10304, guid: 0000000000000000f000000000000000, type: 0} + m_HaloStrength: 0.5 + m_FlareStrength: 1 + m_FlareFadeSpeed: 3 + m_HaloTexture: {fileID: 0} + m_SpotCookie: {fileID: 10001, guid: 0000000000000000e000000000000000, type: 0} + m_DefaultReflectionMode: 0 + m_DefaultReflectionResolution: 128 + m_ReflectionBounces: 1 + m_ReflectionIntensity: 1 + m_CustomReflection: {fileID: 0} + m_Sun: {fileID: 705507994} + m_IndirectSpecularColor: {r: 0.18028378, g: 0.22571412, b: 0.30692285, a: 1} + m_UseRadianceAmbientProbe: 0 +--- !u!157 &3 +LightmapSettings: + m_ObjectHideFlags: 0 + serializedVersion: 12 + m_GIWorkflowMode: 1 + m_GISettings: + serializedVersion: 2 + m_BounceScale: 1 + m_IndirectOutputScale: 1 + m_AlbedoBoost: 1 + m_EnvironmentLightingMode: 0 + m_EnableBakedLightmaps: 1 + m_EnableRealtimeLightmaps: 0 + m_LightmapEditorSettings: + serializedVersion: 12 + m_Resolution: 2 + m_BakeResolution: 40 + m_AtlasSize: 1024 + m_AO: 0 + m_AOMaxDistance: 1 + m_CompAOExponent: 1 + m_CompAOExponentDirect: 0 + m_ExtractAmbientOcclusion: 0 + m_Padding: 2 + m_LightmapParameters: {fileID: 0} + m_LightmapsBakeMode: 1 + m_TextureCompression: 1 + m_FinalGather: 0 + m_FinalGatherFiltering: 1 + m_FinalGatherRayCount: 256 + m_ReflectionCompression: 2 + m_MixedBakeMode: 2 + m_BakeBackend: 1 + m_PVRSampling: 1 + m_PVRDirectSampleCount: 32 + m_PVRSampleCount: 500 + m_PVRBounces: 2 + m_PVREnvironmentSampleCount: 500 + m_PVREnvironmentReferencePointCount: 2048 + m_PVRFilteringMode: 2 + m_PVRDenoiserTypeDirect: 0 + m_PVRDenoiserTypeIndirect: 0 + m_PVRDenoiserTypeAO: 0 + m_PVRFilterTypeDirect: 0 + m_PVRFilterTypeIndirect: 0 + m_PVRFilterTypeAO: 0 + m_PVREnvironmentMIS: 0 + m_PVRCulling: 1 + m_PVRFilteringGaussRadiusDirect: 1 + m_PVRFilteringGaussRadiusIndirect: 5 + m_PVRFilteringGaussRadiusAO: 2 + m_PVRFilteringAtrousPositionSigmaDirect: 0.5 + m_PVRFilteringAtrousPositionSigmaIndirect: 2 + m_PVRFilteringAtrousPositionSigmaAO: 1 + m_ExportTrainingData: 0 + m_TrainingDataDestination: TrainingData + m_LightProbeSampleCountMultiplier: 4 + m_LightingDataAsset: {fileID: 0} + m_LightingSettings: {fileID: 0} +--- !u!196 &4 +NavMeshSettings: + serializedVersion: 2 + m_ObjectHideFlags: 0 + m_BuildSettings: + serializedVersion: 2 + agentTypeID: 0 + agentRadius: 0.5 + agentHeight: 2 + agentSlope: 45 + agentClimb: 0.4 + ledgeDropHeight: 0 + maxJumpAcrossDistance: 0 + minRegionArea: 2 + manualCellSize: 0 + cellSize: 0.16666667 + manualTileSize: 0 + tileSize: 256 + accuratePlacement: 0 + maxJobWorkers: 0 + preserveTilesOutsideBounds: 0 + debug: + m_Flags: 0 + m_NavMeshData: {fileID: 0} +--- !u!1 &164078147 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 164078151} + - component: {fileID: 164078150} + - component: {fileID: 164078149} + - component: {fileID: 164078148} + m_Layer: 0 + m_Name: ROS + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!114 &164078148 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 164078147} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: e60b6d8e41799f649a5310186e7e877f, type: 3} + m_Name: + m_EditorClassIdentifier: + Topic: add_two_ints + Timeout: 1 +--- !u!114 &164078149 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 164078147} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 982645727d5e170429d5ee04bc9b052f, type: 3} + m_Name: + m_EditorClassIdentifier: + Topic: add_two_ints +--- !u!114 &164078150 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 164078147} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: feab04ad06492965492b3edc6423aa53, type: 3} + m_Name: + m_EditorClassIdentifier: +--- !u!4 &164078151 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 164078147} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &705507993 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 705507995} + - component: {fileID: 705507994} + m_Layer: 0 + m_Name: Directional Light + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!108 &705507994 +Light: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 705507993} + m_Enabled: 1 + serializedVersion: 10 + m_Type: 1 + m_Shape: 0 + m_Color: {r: 1, g: 0.95686275, b: 0.8392157, a: 1} + m_Intensity: 1 + m_Range: 10 + m_SpotAngle: 30 + m_InnerSpotAngle: 21.80208 + m_CookieSize: 10 + m_Shadows: + m_Type: 2 + m_Resolution: -1 + m_CustomResolution: -1 + m_Strength: 1 + m_Bias: 0.05 + m_NormalBias: 0.4 + m_NearPlane: 0.2 + m_CullingMatrixOverride: + e00: 1 + e01: 0 + e02: 0 + e03: 0 + e10: 0 + e11: 1 + e12: 0 + e13: 0 + e20: 0 + e21: 0 + e22: 1 + e23: 0 + e30: 0 + e31: 0 + e32: 0 + e33: 1 + m_UseCullingMatrixOverride: 0 + m_Cookie: {fileID: 0} + m_DrawHalo: 0 + m_Flare: {fileID: 0} + m_RenderMode: 0 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingLayerMask: 1 + m_Lightmapping: 1 + m_LightShadowCasterMode: 0 + m_AreaSize: {x: 1, y: 1} + m_BounceIntensity: 1 + m_ColorTemperature: 6570 + m_UseColorTemperature: 0 + m_BoundingSphereOverride: {x: 0, y: 0, z: 0, w: 0} + m_UseBoundingSphereOverride: 0 + m_UseViewFrustumForShadowCasterCull: 1 + m_ShadowRadius: 0 + m_ShadowAngle: 0 +--- !u!4 &705507995 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 705507993} + m_LocalRotation: {x: 0.40821788, y: -0.23456968, z: 0.10938163, w: 0.8754261} + m_LocalPosition: {x: 0, y: 3, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0} +--- !u!1 &963194225 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 963194228} + - component: {fileID: 963194227} + - component: {fileID: 963194226} + m_Layer: 0 + m_Name: Main Camera + m_TagString: MainCamera + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!81 &963194226 +AudioListener: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 963194225} + m_Enabled: 1 +--- !u!20 &963194227 +Camera: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 963194225} + m_Enabled: 1 + serializedVersion: 2 + m_ClearFlags: 1 + m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0} + m_projectionMatrixMode: 1 + m_GateFitMode: 2 + m_FOVAxisMode: 0 + m_SensorSize: {x: 36, y: 24} + m_LensShift: {x: 0, y: 0} + m_FocalLength: 50 + m_NormalizedViewPortRect: + serializedVersion: 2 + x: 0 + y: 0 + width: 1 + height: 1 + near clip plane: 0.3 + far clip plane: 1000 + field of view: 60 + orthographic: 0 + orthographic size: 5 + m_Depth: -1 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingPath: -1 + m_TargetTexture: {fileID: 0} + m_TargetDisplay: 0 + m_TargetEye: 3 + m_HDR: 1 + m_AllowMSAA: 1 + m_AllowDynamicResolution: 0 + m_ForceIntoRT: 0 + m_OcclusionCulling: 1 + m_StereoConvergence: 10 + m_StereoSeparation: 0.022 +--- !u!4 &963194228 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 963194225} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 1, z: -10} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} diff --git a/src/Ros2ForUnity/Samples~/Clients and Services/Scenes/SampleScene.unity.meta b/src/Ros2ForUnity/Samples~/Clients and Services/Scenes/SampleScene.unity.meta new file mode 100644 index 0000000..952bd1e --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scenes/SampleScene.unity.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 9fc0d4010bbf28b4594072e72b8655ab +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts.meta b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts.meta similarity index 77% rename from src/Ros2ForUnity/Scripts.meta rename to src/Ros2ForUnity/Samples~/Clients and Services/Scripts.meta index fb3bbc9..8872b7f 100644 --- a/src/Ros2ForUnity/Scripts.meta +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: f750980d49c8bcf39830e89365689d16 +guid: 2fb0babfbf583d944b31cf87bab0e949 folderAsset: yes DefaultImporter: externalObjects: {} diff --git a/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ClientExample.cs b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ClientExample.cs new file mode 100644 index 0000000..3ec77a4 --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ClientExample.cs @@ -0,0 +1,93 @@ +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using System.Collections; +using System.Threading.Tasks; +using UnityEngine; +using ROS2; + +using Request = example_interfaces.srv.AddTwoInts_Request; +using Response = example_interfaces.srv.AddTwoInts_Response; + + +/// +/// An example class provided for testing of basic ROS2 client +/// +public class ClientExample : MonoBehaviour +{ + private const string NODE_NAME = "client_node"; + + /// + /// Service topic. + /// + public string Topic = "add_two_ints"; + + /// + /// Timeout for requests. + /// + public float Timeout = 1; + + private ROS2UnityComponent ROS; + + private ROS2Node Node; + + private IClient Client; + + /// + /// Create the client. + /// + void Start() + { + this.ROS = GetComponent(); + this.Node = this.ROS.CreateNode(NODE_NAME); + this.Client = this.Node.CreateClient(this.Topic); + this.StartCoroutine(this.RequestAnswers()); + } + + /// + /// Wait for the service to become available + /// and send random requests. + /// + private IEnumerator RequestAnswers() + { + while (!this.Client.IsServiceAvailable()) + { + Debug.Log("Waiting for Service"); + yield return new WaitForSecondsRealtime(0.25f); + } + + while (this.ROS.Ok()) + { + var request = new Request() { A = Random.Range(0, 100), B = Random.Range(0, 100) }; + + Debug.Log($"Request answer for {request.A} + {request.B}"); + using (Task task = this.Client.CallAsync(request)) + { + float deadline = Time.time + this.Timeout; + yield return new WaitUntil(() => task.IsCompleted || Time.time >= deadline); + + if (task.IsCompleted) + { + Debug.Log($"Received answer {task.Result.Sum}"); + Debug.Assert(task.Result.Sum == request.A + request.B, "Received invalid answer"); + } + else + { + Debug.LogError($"Service call timed out"); + this.Client.Cancel(task); + } + } + } + } +} diff --git a/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ClientExample.cs.meta b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ClientExample.cs.meta new file mode 100644 index 0000000..f68821a --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ClientExample.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e60b6d8e41799f649a5310186e7e877f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ServiceExample.cs b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ServiceExample.cs new file mode 100644 index 0000000..6e80964 --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ServiceExample.cs @@ -0,0 +1,54 @@ +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using UnityEngine; +using ROS2; + +using Request = example_interfaces.srv.AddTwoInts_Request; +using Response = example_interfaces.srv.AddTwoInts_Response; + +/// +/// An example class provided for testing of basic ROS2 service +/// +public class ServiceExample : MonoBehaviour +{ + private const string NODE_NAME = "service_node"; + + /// + /// Topic of the service. + /// + public string Topic = "add_two_ints"; + + private ROS2UnityComponent ROS; + + private ROS2Node Node; + + private IService Service; + + /// + /// Create the service. + /// + void Start() + { + this.ROS = GetComponent(); + this.Node = this.ROS.CreateNode(NODE_NAME); + this.Service = this.Node.CreateService(this.Topic, this.OnRequest); + } + + private Response OnRequest(Request msg) + { + Debug.Log($"Incoming Service Request A={msg.A} B={msg.B}"); + return new Response() { Sum = msg.A + msg.B }; + } +} diff --git a/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ServiceExample.cs.meta b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ServiceExample.cs.meta new file mode 100644 index 0000000..e9c68a5 --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Clients and Services/Scripts/ServiceExample.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 982645727d5e170429d5ee04bc9b052f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes.meta b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes.meta new file mode 100644 index 0000000..83c741b --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 6ea315d0fd7389c41b19996891e99ae3 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes/SampleScene.unity b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes/SampleScene.unity new file mode 100644 index 0000000..5200df1 --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes/SampleScene.unity @@ -0,0 +1,370 @@ +%YAML 1.1 +%TAG !u! tag:unity3d.com,2011: +--- !u!29 &1 +OcclusionCullingSettings: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_OcclusionBakeSettings: + smallestOccluder: 5 + smallestHole: 0.25 + backfaceThreshold: 100 + m_SceneGUID: 00000000000000000000000000000000 + m_OcclusionCullingData: {fileID: 0} +--- !u!104 &2 +RenderSettings: + m_ObjectHideFlags: 0 + serializedVersion: 9 + m_Fog: 0 + m_FogColor: {r: 0.5, g: 0.5, b: 0.5, a: 1} + m_FogMode: 3 + m_FogDensity: 0.01 + m_LinearFogStart: 0 + m_LinearFogEnd: 300 + m_AmbientSkyColor: {r: 0.212, g: 0.227, b: 0.259, a: 1} + m_AmbientEquatorColor: {r: 0.114, g: 0.125, b: 0.133, a: 1} + m_AmbientGroundColor: {r: 0.047, g: 0.043, b: 0.035, a: 1} + m_AmbientIntensity: 1 + m_AmbientMode: 0 + m_SubtractiveShadowColor: {r: 0.42, g: 0.478, b: 0.627, a: 1} + m_SkyboxMaterial: {fileID: 10304, guid: 0000000000000000f000000000000000, type: 0} + m_HaloStrength: 0.5 + m_FlareStrength: 1 + m_FlareFadeSpeed: 3 + m_HaloTexture: {fileID: 0} + m_SpotCookie: {fileID: 10001, guid: 0000000000000000e000000000000000, type: 0} + m_DefaultReflectionMode: 0 + m_DefaultReflectionResolution: 128 + m_ReflectionBounces: 1 + m_ReflectionIntensity: 1 + m_CustomReflection: {fileID: 0} + m_Sun: {fileID: 705507994} + m_IndirectSpecularColor: {r: 0.18028378, g: 0.22571412, b: 0.30692285, a: 1} + m_UseRadianceAmbientProbe: 0 +--- !u!157 &3 +LightmapSettings: + m_ObjectHideFlags: 0 + serializedVersion: 12 + m_GIWorkflowMode: 1 + m_GISettings: + serializedVersion: 2 + m_BounceScale: 1 + m_IndirectOutputScale: 1 + m_AlbedoBoost: 1 + m_EnvironmentLightingMode: 0 + m_EnableBakedLightmaps: 1 + m_EnableRealtimeLightmaps: 0 + m_LightmapEditorSettings: + serializedVersion: 12 + m_Resolution: 2 + m_BakeResolution: 40 + m_AtlasSize: 1024 + m_AO: 0 + m_AOMaxDistance: 1 + m_CompAOExponent: 1 + m_CompAOExponentDirect: 0 + m_ExtractAmbientOcclusion: 0 + m_Padding: 2 + m_LightmapParameters: {fileID: 0} + m_LightmapsBakeMode: 1 + m_TextureCompression: 1 + m_FinalGather: 0 + m_FinalGatherFiltering: 1 + m_FinalGatherRayCount: 256 + m_ReflectionCompression: 2 + m_MixedBakeMode: 2 + m_BakeBackend: 1 + m_PVRSampling: 1 + m_PVRDirectSampleCount: 32 + m_PVRSampleCount: 500 + m_PVRBounces: 2 + m_PVREnvironmentSampleCount: 500 + m_PVREnvironmentReferencePointCount: 2048 + m_PVRFilteringMode: 2 + m_PVRDenoiserTypeDirect: 0 + m_PVRDenoiserTypeIndirect: 0 + m_PVRDenoiserTypeAO: 0 + m_PVRFilterTypeDirect: 0 + m_PVRFilterTypeIndirect: 0 + m_PVRFilterTypeAO: 0 + m_PVREnvironmentMIS: 0 + m_PVRCulling: 1 + m_PVRFilteringGaussRadiusDirect: 1 + m_PVRFilteringGaussRadiusIndirect: 5 + m_PVRFilteringGaussRadiusAO: 2 + m_PVRFilteringAtrousPositionSigmaDirect: 0.5 + m_PVRFilteringAtrousPositionSigmaIndirect: 2 + m_PVRFilteringAtrousPositionSigmaAO: 1 + m_ExportTrainingData: 0 + m_TrainingDataDestination: TrainingData + m_LightProbeSampleCountMultiplier: 4 + m_LightingDataAsset: {fileID: 0} + m_LightingSettings: {fileID: 0} +--- !u!196 &4 +NavMeshSettings: + serializedVersion: 2 + m_ObjectHideFlags: 0 + m_BuildSettings: + serializedVersion: 2 + agentTypeID: 0 + agentRadius: 0.5 + agentHeight: 2 + agentSlope: 45 + agentClimb: 0.4 + ledgeDropHeight: 0 + maxJumpAcrossDistance: 0 + minRegionArea: 2 + manualCellSize: 0 + cellSize: 0.16666667 + manualTileSize: 0 + tileSize: 256 + accuratePlacement: 0 + maxJobWorkers: 0 + preserveTilesOutsideBounds: 0 + debug: + m_Flags: 0 + m_NavMeshData: {fileID: 0} +--- !u!1 &705507993 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 705507995} + - component: {fileID: 705507994} + m_Layer: 0 + m_Name: Directional Light + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!108 &705507994 +Light: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 705507993} + m_Enabled: 1 + serializedVersion: 10 + m_Type: 1 + m_Shape: 0 + m_Color: {r: 1, g: 0.95686275, b: 0.8392157, a: 1} + m_Intensity: 1 + m_Range: 10 + m_SpotAngle: 30 + m_InnerSpotAngle: 21.80208 + m_CookieSize: 10 + m_Shadows: + m_Type: 2 + m_Resolution: -1 + m_CustomResolution: -1 + m_Strength: 1 + m_Bias: 0.05 + m_NormalBias: 0.4 + m_NearPlane: 0.2 + m_CullingMatrixOverride: + e00: 1 + e01: 0 + e02: 0 + e03: 0 + e10: 0 + e11: 1 + e12: 0 + e13: 0 + e20: 0 + e21: 0 + e22: 1 + e23: 0 + e30: 0 + e31: 0 + e32: 0 + e33: 1 + m_UseCullingMatrixOverride: 0 + m_Cookie: {fileID: 0} + m_DrawHalo: 0 + m_Flare: {fileID: 0} + m_RenderMode: 0 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingLayerMask: 1 + m_Lightmapping: 1 + m_LightShadowCasterMode: 0 + m_AreaSize: {x: 1, y: 1} + m_BounceIntensity: 1 + m_ColorTemperature: 6570 + m_UseColorTemperature: 0 + m_BoundingSphereOverride: {x: 0, y: 0, z: 0, w: 0} + m_UseBoundingSphereOverride: 0 + m_UseViewFrustumForShadowCasterCull: 1 + m_ShadowRadius: 0 + m_ShadowAngle: 0 +--- !u!4 &705507995 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 705507993} + m_LocalRotation: {x: 0.40821788, y: -0.23456968, z: 0.10938163, w: 0.8754261} + m_LocalPosition: {x: 0, y: 3, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 1 + m_LocalEulerAnglesHint: {x: 50, y: -30, z: 0} +--- !u!1 &963194225 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 963194228} + - component: {fileID: 963194227} + - component: {fileID: 963194226} + m_Layer: 0 + m_Name: Main Camera + m_TagString: MainCamera + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!81 &963194226 +AudioListener: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 963194225} + m_Enabled: 1 +--- !u!20 &963194227 +Camera: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 963194225} + m_Enabled: 1 + serializedVersion: 2 + m_ClearFlags: 1 + m_BackGroundColor: {r: 0.19215687, g: 0.3019608, b: 0.4745098, a: 0} + m_projectionMatrixMode: 1 + m_GateFitMode: 2 + m_FOVAxisMode: 0 + m_SensorSize: {x: 36, y: 24} + m_LensShift: {x: 0, y: 0} + m_FocalLength: 50 + m_NormalizedViewPortRect: + serializedVersion: 2 + x: 0 + y: 0 + width: 1 + height: 1 + near clip plane: 0.3 + far clip plane: 1000 + field of view: 60 + orthographic: 0 + orthographic size: 5 + m_Depth: -1 + m_CullingMask: + serializedVersion: 2 + m_Bits: 4294967295 + m_RenderingPath: -1 + m_TargetTexture: {fileID: 0} + m_TargetDisplay: 0 + m_TargetEye: 3 + m_HDR: 1 + m_AllowMSAA: 1 + m_AllowDynamicResolution: 0 + m_ForceIntoRT: 0 + m_OcclusionCulling: 1 + m_StereoConvergence: 10 + m_StereoSeparation: 0.022 +--- !u!4 &963194228 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 963194225} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 1, z: -10} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &989546317 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 989546321} + - component: {fileID: 989546320} + - component: {fileID: 989546319} + - component: {fileID: 989546318} + m_Layer: 0 + m_Name: ROS + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!114 &989546318 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 989546317} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 7d5265ec9446ab94c901b5ee1511e905, type: 3} + m_Name: + m_EditorClassIdentifier: +--- !u!114 &989546319 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 989546317} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: c7ca536f8b4f8c04eb6dfdae42d7327e, type: 3} + m_Name: + m_EditorClassIdentifier: +--- !u!114 &989546320 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 989546317} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: feab04ad06492965492b3edc6423aa53, type: 3} + m_Name: + m_EditorClassIdentifier: +--- !u!4 &989546321 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 989546317} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 1, y: 1, z: 1} + m_Children: [] + m_Father: {fileID: 0} + m_RootOrder: 2 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} diff --git a/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes/SampleScene.unity.meta b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes/SampleScene.unity.meta new file mode 100644 index 0000000..952bd1e --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scenes/SampleScene.unity.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 9fc0d4010bbf28b4594072e72b8655ab +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts.meta b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts.meta new file mode 100644 index 0000000..dc0345d --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b5b3399cae737434fa2f98d924dbb35b +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/PublisherExample.cs b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/PublisherExample.cs new file mode 100644 index 0000000..3d5c015 --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/PublisherExample.cs @@ -0,0 +1,60 @@ +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using UnityEngine; +using ROS2; + +/// +/// An example class provided for testing of basic ROS2 communication +/// +public class PublisherExample : MonoBehaviour +{ + private const string NODE_NAME = "talker_node"; + + /// + /// Topic to publish to. + /// + public string Topic = "chatter"; + + private ROS2UnityComponent ROS; + + private ROS2Node Node; + + private IPublisher Publisher; + + private int i = 0; + + /// + /// Create the publisher. + /// + void Start() + { + this.ROS = GetComponent(); + this.Node = this.ROS.CreateNode(NODE_NAME); + this.Publisher = this.Node.CreatePublisher("chatter"); + } + + /// + /// Publish a new message. + /// + void Update() + { + if (this.ROS.Ok()) + { + this.i++; + var msg = new std_msgs.msg.String() { Data = $"Unity ROS2 sending: hello {this.i}" }; + this.Publisher.Publish(msg); + } + } +} diff --git a/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/PublisherExample.cs.meta b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/PublisherExample.cs.meta new file mode 100644 index 0000000..e774488 --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/PublisherExample.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c7ca536f8b4f8c04eb6dfdae42d7327e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/ROS2ListenerExample.cs b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/SubscriptionExample.cs similarity index 50% rename from src/Ros2ForUnity/Scripts/ROS2ListenerExample.cs rename to src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/SubscriptionExample.cs index d145359..5c41249 100644 --- a/src/Ros2ForUnity/Scripts/ROS2ListenerExample.cs +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/SubscriptionExample.cs @@ -1,46 +1,50 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using System; -using UnityEngine; - -namespace ROS2 -{ - -/// -/// An example class provided for testing of basic ROS2 communication -/// -public class ROS2ListenerExample : MonoBehaviour -{ - private ROS2UnityComponent ros2Unity; - private ROS2Node ros2Node; - private ISubscription chatter_sub; - - void Start() - { - ros2Unity = GetComponent(); - } - - void Update() - { - if (ros2Node == null && ros2Unity.Ok()) - { - ros2Node = ros2Unity.CreateNode("ROS2UnityListenerNode"); - chatter_sub = ros2Node.CreateSubscription( - "chatter", msg => Debug.Log("Unity listener heard: [" + msg.Data + "]")); - } - } -} - -} // namespace ROS2 +// Copyright 2019-2021 Robotec.ai. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +using UnityEngine; +using ROS2; + +/// +/// An example class provided for testing of basic ROS2 communication +/// +public class SubscriptionExample : MonoBehaviour +{ + private const string NODE_NAME = "listener_node"; + + /// + /// Topic to listen on. + /// + public string Topic = "chatter"; + + private ROS2UnityComponent ROS; + + private ROS2Node Node; + + private ISubscription Subscription; + + /// + /// Create the subscription. + /// + void Start() + { + this.ROS = GetComponent(); + this.Node = this.ROS.CreateNode(NODE_NAME); + this.Subscription = this.Node.CreateSubscription(this.Topic, this.OnMessage); + } + + private void OnMessage(std_msgs.msg.String msg) + { + Debug.Log($"Unity listener heard: [{msg.Data}]"); + } +} diff --git a/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/SubscriptionExample.cs.meta b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/SubscriptionExample.cs.meta new file mode 100644 index 0000000..af8e012 --- /dev/null +++ b/src/Ros2ForUnity/Samples~/Publishers and Subscriptions/Scripts/SubscriptionExample.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7d5265ec9446ab94c901b5ee1511e905 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/Ros2ForUnity/Scripts/PostInstall.cs b/src/Ros2ForUnity/Scripts/PostInstall.cs deleted file mode 100644 index 8647e7f..0000000 --- a/src/Ros2ForUnity/Scripts/PostInstall.cs +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2019-2022 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#if UNITY_EDITOR -using System; -using System.Collections.Generic; -using System.IO; -using System.Linq; -using System.Text.RegularExpressions; -using UnityEngine; -using UnityEditor; -using UnityEditor.Build; -using UnityEditor.Build.Reporting; - -namespace ROS2 -{ - -/// -/// An internal class responsible for installing ros2-for-unity metadata files -/// -internal class PostInstall : IPostprocessBuildWithReport -{ - public int callbackOrder { get { return 0; } } - public void OnPostprocessBuild(BuildReport report) - { - var r2fuMetadataName = "metadata_ros2_for_unity.xml"; - var r2csMetadataName = "metadata_ros2cs.xml"; - - // FileUtil.CopyFileOrDirectory: All file separators should be forward ones "/". - var r2fuMeta = ROS2ForUnity.GetRos2ForUnityPath() + "/" + r2fuMetadataName; - var r2csMeta = ROS2ForUnity.GetPluginPath() + "/" + r2csMetadataName; - var outputDir = Directory.GetParent(report.summary.outputPath); - var execFilename = Path.GetFileNameWithoutExtension(report.summary.outputPath); - FileUtil.CopyFileOrDirectory( - r2fuMeta, outputDir + "/" + execFilename + "_Data/" + r2fuMetadataName); - if (EditorUserBuildSettings.activeBuildTarget == BuildTarget.StandaloneLinux64) { - FileUtil.CopyFileOrDirectory( - r2csMeta, outputDir + "/" + execFilename + "_Data/Plugins/" + r2csMetadataName); - - // Copy versioned libraries (Unity skips them) - Regex soWithVersionReg = new Regex(@".*\.so(\.[0-9])+$"); - var versionedLibs = new List(Directory.GetFiles(ROS2ForUnity.GetPluginPath())) - .Where(path => soWithVersionReg.IsMatch(path)) - .ToList(); - foreach (var libPath in versionedLibs) { - FileUtil.CopyFileOrDirectory( - libPath, outputDir + "/" + execFilename + "_Data/Plugins/" + Path.GetFileName(libPath)); - } - } else { - FileUtil.CopyFileOrDirectory( - r2csMeta, outputDir + "/" + execFilename + "_Data/Plugins/x86_64/" + r2csMetadataName); - } - } - -} - -} -#endif diff --git a/src/Ros2ForUnity/Scripts/ROS2ClientExample.cs b/src/Ros2ForUnity/Scripts/ROS2ClientExample.cs deleted file mode 100644 index 2c8e274..0000000 --- a/src/Ros2ForUnity/Scripts/ROS2ClientExample.cs +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using System.Collections; -using System.Collections.Generic; -using System.Threading.Tasks; -using UnityEngine; -using ROS2; - -using addTwoIntsReq = example_interfaces.srv.AddTwoInts_Request; -using addTwoIntsResp = example_interfaces.srv.AddTwoInts_Response; - -/// -/// An example class provided for testing of basic ROS2 client -/// -public class ROS2ClientExample : MonoBehaviour -{ - private ROS2UnityComponent ros2Unity; - private ROS2Node ros2Node; - private IClient addTwoIntsClient; - private bool isRunning = false; - private Task asyncTask; - - IEnumerator periodicAsyncCall() - { - while (ros2Unity.Ok()) - { - - while (!addTwoIntsClient.IsServiceAvailable()) - { - yield return new WaitForSecondsRealtime(1); - } - - addTwoIntsReq request = new addTwoIntsReq(); - request.A = Random.Range(0, 100); - request.B = Random.Range(0, 100); - - asyncTask = addTwoIntsClient.CallAsync(request); - asyncTask.ContinueWith((task) => { Debug.Log("Got async answer " + task.Result.Sum); }); - - yield return new WaitForSecondsRealtime(1); - } - } - - IEnumerator periodicCall() - { - while (ros2Unity.Ok()) - { - - while (!addTwoIntsClient.IsServiceAvailable()) - { - yield return new WaitForSecondsRealtime(1); - } - - addTwoIntsReq request = new addTwoIntsReq(); - request.A = Random.Range(0, 100); - request.B = Random.Range(0, 100); - var response = addTwoIntsClient.Call(request); - - Debug.Log("Got sync answer " + response.Sum); - - yield return new WaitForSecondsRealtime(1); - } - } - - void Start() - { - ros2Unity = GetComponent(); - if (ros2Unity.Ok()) - { - if (ros2Node == null) - { - ros2Node = ros2Unity.CreateNode("ROS2UnityClient"); - addTwoIntsClient = ros2Node.CreateClient( - "add_two_ints"); - } - } - } - - void Update() - { - if (!isRunning) - { - isRunning = true; - - // Async calls - StartCoroutine(periodicAsyncCall()); - - // Sync calls - StartCoroutine(periodicCall()); - } - } -} diff --git a/src/Ros2ForUnity/Scripts/ROS2ForUnity.cs b/src/Ros2ForUnity/Scripts/ROS2ForUnity.cs deleted file mode 100644 index 4ba7cfd..0000000 --- a/src/Ros2ForUnity/Scripts/ROS2ForUnity.cs +++ /dev/null @@ -1,372 +0,0 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using System; -using System.IO; -using System.Collections.Generic; -using UnityEngine; -using UnityEditor; -using System.Xml; - -namespace ROS2 -{ - -/// -/// An internal class responsible for handling checking, proper initialization and shutdown of ROS2cs, -/// -internal class ROS2ForUnity -{ - private static bool isInitialized = false; - private static string ros2ForUnityAssetFolderName = "Ros2ForUnity"; - private XmlDocument ros2csMetadata = new XmlDocument(); - private XmlDocument ros2ForUnityMetadata = new XmlDocument(); - - public enum Platform - { - Windows, - Linux - } - - public static Platform GetOS() - { - if (Application.platform == RuntimePlatform.LinuxEditor || Application.platform == RuntimePlatform.LinuxPlayer) - { - return Platform.Linux; - } - else if (Application.platform == RuntimePlatform.WindowsEditor || Application.platform == RuntimePlatform.WindowsPlayer) - { - return Platform.Windows; - } - throw new System.NotSupportedException("Only Linux and Windows are supported"); - } - - private static bool InEditor() { - return Application.isEditor; - } - - private static string GetOSName() - { - switch (GetOS()) - { - case Platform.Linux: - return "Linux"; - case Platform.Windows: - return "Windows"; - default: - throw new System.NotSupportedException("Only Linux and Windows are supported"); - } - } - - private string GetEnvPathVariableName() - { - string envVariable = "LD_LIBRARY_PATH"; - if (GetOS() == Platform.Windows) - { - envVariable = "PATH"; - } - return envVariable; - } - - private string GetEnvPathVariableValue() - { - return Environment.GetEnvironmentVariable(GetEnvPathVariableName()); - } - - public static string GetRos2ForUnityPath() - { - char separator = Path.DirectorySeparatorChar; - string appDataPath = Application.dataPath; - string pluginPath = appDataPath; - - if (InEditor()) { - pluginPath += separator + ros2ForUnityAssetFolderName; - } - return pluginPath; - } - - public static string GetPluginPath() - { - char separator = Path.DirectorySeparatorChar; - string ros2ForUnityPath = GetRos2ForUnityPath(); - string pluginPath = ros2ForUnityPath; - - pluginPath += separator + "Plugins"; - - if (InEditor()) { - pluginPath += separator + GetOSName(); - } - - if (InEditor() || GetOS() == Platform.Windows) - { - pluginPath += separator + "x86_64"; - } - - if (GetOS() == Platform.Windows) - { - pluginPath = pluginPath.Replace("/", "\\"); - } - - return pluginPath; - } - - /// - /// Function responsible for setting up of environment paths for standalone builds - /// - /// - /// Note that on Linux, LD_LIBRARY_PATH as used for dlopen() is determined on process start and this change won't - /// affect it. Ros2 looks for rmw implementation based on this variable (independently) and the change - /// is effective for this process, however rmw implementation's dependencies itself are loaded by dynamic linker - /// anyway so setting it for Linux is pointless. - /// - private void SetEnvPathVariable() - { - string currentPath = GetEnvPathVariableValue(); - string pluginPath = GetPluginPath(); - - char envPathSep = ':'; - if (GetOS() == Platform.Windows) - { - envPathSep = ';'; - } - - Environment.SetEnvironmentVariable(GetEnvPathVariableName(), pluginPath + envPathSep + currentPath); - } - - public bool IsStandalone() { - return Convert.ToBoolean(Convert.ToInt16(GetMetadataValue(ros2csMetadata, "/ros2cs/standalone"))); - } - - public string GetROSVersion() - { - string ros2SourcedCodename = GetROSVersionSourced(); - string ros2FromRos4UMetadata = GetMetadataValue(ros2ForUnityMetadata, "/ros2_for_unity/ros2"); - - // Sourced ROS2 libs takes priority - if (string.IsNullOrEmpty(ros2SourcedCodename)) { - return ros2FromRos4UMetadata; - } - - return ros2SourcedCodename; - } - - /// - /// Checks if both ros2cs and ros2-for-unity were build for the same ros version as well as - /// the current sourced ros version matches ros2cs binaries. - /// - public void CheckIntegrity() - { - string ros2SourcedCodename = GetROSVersionSourced(); - string ros2FromRos2csMetadata = GetMetadataValue(ros2csMetadata, "/ros2cs/ros2"); - string ros2FromRos4UMetadata = GetMetadataValue(ros2ForUnityMetadata, "/ros2_for_unity/ros2"); - - if (ros2FromRos4UMetadata != ros2FromRos2csMetadata) { - Debug.LogError( - "ROS2 versions in 'ros2cs' and 'ros2-for-unity' metadata files are not the same. " + - "This is caused by mixing versions/builds. Plugin might not work correctly." - ); - } - - if(!IsStandalone() && ros2SourcedCodename != ros2FromRos2csMetadata) { - Debug.LogError( - "ROS2 version in 'ros2cs' metadata doesn't match currently sourced version. " + - "This is caused by mixing versions/builds. Plugin might not work correctly." - ); - } - - if (IsStandalone() && !string.IsNullOrEmpty(ros2SourcedCodename)) { - Debug.LogError( - "You should not source ROS2 in 'ros2-for-unity' standalone build. " + - "Plugin might not work correctly." - ); - } - } - - public string GetROSVersionSourced() - { - return Environment.GetEnvironmentVariable("ROS_DISTRO"); - } - - /// - /// Check if the ros version is supported, only applicable to non-standalone plugin versions - /// (i. e. without ros2 libraries included in the plugin). - /// - private void CheckROSSupport(string ros2Codename) - { - List supportedVersions = new List() { "foxy", "galactic", "humble", "rolling" }; - var supportedVersionsString = String.Join(", ", supportedVersions); - if (string.IsNullOrEmpty(ros2Codename)) - { - string errMessage = "No ROS environment sourced. You need to source your ROS2 " + supportedVersionsString - + " environment before launching Unity (ROS_DISTRO env variable not found)"; - Debug.LogError(errMessage); -#if UNITY_EDITOR - EditorApplication.isPlaying = false; - throw new System.InvalidOperationException(errMessage); -#else - const int ROS_NOT_SOURCED_ERROR_CODE = 33; - Application.Quit(ROS_NOT_SOURCED_ERROR_CODE); -#endif - } - - if (!supportedVersions.Contains(ros2Codename)) - { - string errMessage = "Currently sourced ROS version differs from supported one. Sourced: " + ros2Codename - + ", supported: " + supportedVersionsString + "."; - Debug.LogError(errMessage); -#if UNITY_EDITOR - EditorApplication.isPlaying = false; - throw new System.NotSupportedException(errMessage); -#else - const int ROS_BAD_VERSION_CODE = 34; - Application.Quit(ROS_BAD_VERSION_CODE); -#endif - } else if (ros2Codename.Equals("rolling") ) { - Debug.LogWarning("You are using ROS2 rolling version. Bleeding edge version might not work correctly."); - } - } - - private void RegisterCtrlCHandler() - { -#if ENABLE_MONO - // Il2CPP build does not support Console.CancelKeyPress currently - Console.CancelKeyPress += (sender, eventArgs) => { - eventArgs.Cancel = true; - DestroyROS2ForUnity(); - }; -#endif - } - - private void ConnectLoggers() - { - Ros2csLogger.setCallback(LogLevel.ERROR, Debug.LogError); - Ros2csLogger.setCallback(LogLevel.WARNING, Debug.LogWarning); - Ros2csLogger.setCallback(LogLevel.INFO, Debug.Log); - Ros2csLogger.setCallback(LogLevel.DEBUG, Debug.Log); - Ros2csLogger.LogLevel = LogLevel.WARNING; - } - - private string GetMetadataValue(XmlDocument doc, string valuePath) - { - return doc.DocumentElement.SelectSingleNode(valuePath).InnerText; - } - - private void LoadMetadata() - { - char separator = Path.DirectorySeparatorChar; - try - { - ros2csMetadata.Load(GetPluginPath() + separator + "metadata_ros2cs.xml"); - ros2ForUnityMetadata.Load(GetRos2ForUnityPath() + separator + "metadata_ros2_for_unity.xml"); - } - catch (System.IO.FileNotFoundException) - { -#if UNITY_EDITOR - var errMessage = "Could not find metadata files."; - EditorApplication.isPlaying = false; - throw new System.IO.FileNotFoundException(errMessage); -#else - const int NO_METADATA = 1; - Application.Quit(NO_METADATA); -#endif - } - } - - internal ROS2ForUnity() - { - // Load metadata - LoadMetadata(); - string currentRos2Version = GetROSVersion(); - string standalone = IsStandalone() ? "standalone" : "non-standalone"; - - // Self checks - CheckROSSupport(currentRos2Version); - CheckIntegrity(); - - // Library loading - if (GetOS() == Platform.Windows) { - // Windows version can run standalone, modifies PATH to ensure all plugins visibility - SetEnvPathVariable(); - } else { - // For foxy, it is necessary to use modified version of librcpputils to resolve custom msgs packages. - ROS2.GlobalVariables.absolutePath = GetPluginPath() + "/"; - if (currentRos2Version == "foxy") { - ROS2.GlobalVariables.preloadLibrary = true; - ROS2.GlobalVariables.preloadLibraryName = "librcpputils.so"; - } - } - - // Initialize - ConnectLoggers(); - Ros2cs.Init(); - RegisterCtrlCHandler(); - - string rmwImpl = Ros2cs.GetRMWImplementation(); - - Debug.Log("ROS2 version: " + currentRos2Version + ". Build type: " + standalone + ". RMW: " + rmwImpl); - -#if UNITY_EDITOR - EditorApplication.playModeStateChanged += this.EditorPlayStateChanged; - EditorApplication.quitting += this.DestroyROS2ForUnity; -#endif - isInitialized = true; - } - - private static void ThrowIfUninitialized(string callContext) - { - if (!isInitialized) - { - throw new InvalidOperationException("Ros2 For Unity is not initialized, can't " + callContext); - } - } - - /// - /// Check if ROS2 module is properly initialized and no shutdown was called yet - /// - /// The state of ROS2 module. Should be checked before attempting to create or use pubs/subs - public bool Ok() - { - if (!isInitialized) - { - return false; - } - return Ros2cs.Ok(); - } - - internal void DestroyROS2ForUnity() - { - if (isInitialized) - { - Debug.Log("Shutting down Ros2 For Unity"); - Ros2cs.Shutdown(); - isInitialized = false; - } - } - - ~ROS2ForUnity() - { - DestroyROS2ForUnity(); - } - -#if UNITY_EDITOR - void EditorPlayStateChanged(PlayModeStateChange change) - { - if (change == PlayModeStateChange.ExitingPlayMode) - { - DestroyROS2ForUnity(); - } - } -#endif -} - -} // namespace ROS2 diff --git a/src/Ros2ForUnity/Scripts/ROS2ServiceExample.cs b/src/Ros2ForUnity/Scripts/ROS2ServiceExample.cs deleted file mode 100644 index 20739ee..0000000 --- a/src/Ros2ForUnity/Scripts/ROS2ServiceExample.cs +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using System.Collections; -using System.Collections.Generic; -using UnityEngine; -using ROS2; - -using addTwoIntsReq = example_interfaces.srv.AddTwoInts_Request; -using addTwoIntsResp = example_interfaces.srv.AddTwoInts_Response; - -/// -/// An example class provided for testing of basic ROS2 service -/// -public class ROS2ServiceExample : MonoBehaviour -{ - private ROS2UnityComponent ros2Unity; - private ROS2Node ros2Node; - private IService addTwoIntsService; - - void Start() - { - ros2Unity = GetComponent(); - if (ros2Unity.Ok()) - { - if (ros2Node == null) - { - ros2Node = ros2Unity.CreateNode("ROS2UnityService"); - addTwoIntsService = ros2Node.CreateService( - "add_two_ints", addTwoInts); - } - } - } - - public example_interfaces.srv.AddTwoInts_Response addTwoInts( example_interfaces.srv.AddTwoInts_Request msg) - { - Debug.Log("Incoming Service Request A=" + msg.A + " B=" + msg.B); - example_interfaces.srv.AddTwoInts_Response response = new example_interfaces.srv.AddTwoInts_Response(); - response.Sum = msg.A + msg.B; - return response; - } -} diff --git a/src/Ros2ForUnity/Scripts/ROS2TalkerExample.cs b/src/Ros2ForUnity/Scripts/ROS2TalkerExample.cs deleted file mode 100644 index 705ea0d..0000000 --- a/src/Ros2ForUnity/Scripts/ROS2TalkerExample.cs +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2019-2021 Robotec.ai. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -using UnityEngine; - -namespace ROS2 -{ - -/// -/// An example class provided for testing of basic ROS2 communication -/// -public class ROS2TalkerExample : MonoBehaviour -{ - // Start is called before the first frame update - private ROS2UnityComponent ros2Unity; - private ROS2Node ros2Node; - private IPublisher chatter_pub; - private int i; - - void Start() - { - ros2Unity = GetComponent(); - } - - void Update() - { - if (ros2Unity.Ok()) - { - if (ros2Node == null) - { - ros2Node = ros2Unity.CreateNode("ROS2UnityTalkerNode"); - chatter_pub = ros2Node.CreatePublisher("chatter"); - } - - i++; - std_msgs.msg.String msg = new std_msgs.msg.String(); - msg.Data = "Unity ROS2 sending: hello " + i; - chatter_pub.Publish(msg); - } - } -} - -} // namespace ROS2 diff --git a/src/Ros2ForUnity/package.json b/src/Ros2ForUnity/package.json new file mode 100644 index 0000000..f2706a1 --- /dev/null +++ b/src/Ros2ForUnity/package.json @@ -0,0 +1,25 @@ +{ + "name": "ai.robotec.ros2-for-unity", + "version": "1.3.0", + "displayName": "Ros2ForUnity", + "description": "Bindings for ROS 2", + "unity": "2020.1", + "license": "Apache License 2.0", + "author": { + "name": "Robotec.ai", + "email": "office@robotec.ai", + "url": "http://www.robotec.ai/" + }, + "samples": [ + { + "displayName": "Publisher and Subscriptions", + "description": "Example on how to use Publishers and Subscriptions", + "path": "Samples~/Publishers and Subscriptions" + }, + { + "displayName": "Clients and Services", + "description": "Example on how to use Clients and Services", + "path": "Samples~/Clients and Services" + } + ] +} \ No newline at end of file diff --git a/src/Ros2ForUnity/package.json.meta b/src/Ros2ForUnity/package.json.meta new file mode 100644 index 0000000..8ab7d06 --- /dev/null +++ b/src/Ros2ForUnity/package.json.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: c3e58271065d3e441b17120275ae4a4c +PackageManifestImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/src/scripts/metadata_generator.py b/src/scripts/metadata_generator.py index 25f6c40..c80a630 100644 --- a/src/scripts/metadata_generator.py +++ b/src/scripts/metadata_generator.py @@ -12,65 +12,27 @@ # See the License for the specific language governing permissions and # limitations under the License. -import argparse -import xml.etree.ElementTree as ET -from xml.dom import minidom -import subprocess -import pathlib import os +from argparse import ArgumentParser +from pathlib import Path +from xml.dom import minidom +from xml.etree import ElementTree as ET -parser = argparse.ArgumentParser(description='Generate metadata file for ros2-for-unity.') -parser.add_argument('--standalone', action='store_true', help='is a standalone build') -args = parser.parse_args() - -def get_git_commit(working_directory) -> str: - return subprocess.check_output(['git', 'rev-parse', 'HEAD'], cwd=working_directory).decode('ascii').strip() - -def get_git_description(working_directory) -> str: - return subprocess.check_output(['git', 'describe', '--tags', '--always'], cwd=working_directory).decode('ascii').strip() - -def get_commit_date(working_directory) -> str: - return subprocess.check_output(['git', 'show', '-s', '--format=%ci'], cwd=working_directory).decode('ascii').strip() - -def get_git_abbrev(working_directory) -> str: - return subprocess.check_output(['git', 'rev-parse', '--abbrev-ref', 'HEAD'], cwd=working_directory).decode('ascii').strip() - -def get_ros2_for_unity_root_path() -> pathlib.Path: - return pathlib.Path(__file__).parents[2] - -def get_ros2_for_unity_path() -> pathlib.Path: - return pathlib.Path(__file__).parents[1].joinpath("Ros2ForUnity") +PARSER = ArgumentParser(description="Generate metadata file for ros2-for-unity.") +PARSER.add_argument("--standalone", action="store_true", help="Whether the build is a standalone build") -def get_ros2cs_path() -> pathlib.Path: - return pathlib.Path(__file__).parents[1].joinpath("ros2cs") +METADATA_PATH = Path(__file__).parents[1] / "Ros2ForUnity" / "Resources" / "ros2-for-unity.xml" -def get_ros2_path() -> pathlib.Path: - return get_ros2cs_path().joinpath("src").joinpath("ros2").joinpath("rcl_interfaces") def get_ros2_version() -> str: return os.environ.get("ROS_DISTRO", "unknown") -ros2_for_unity = ET.Element("ros2_for_unity") -ET.SubElement(ros2_for_unity, "ros2").text = get_ros2_version() -ros2_for_unity_version = ET.SubElement(ros2_for_unity, "version") -ET.SubElement(ros2_for_unity_version, "sha").text = get_git_commit(get_ros2_for_unity_root_path()) -ET.SubElement(ros2_for_unity_version, "desc").text = get_git_description(get_ros2_for_unity_root_path()) -ET.SubElement(ros2_for_unity_version, "date").text = get_commit_date(get_ros2_for_unity_root_path()) - -ros2_cs = ET.Element("ros2cs") -ET.SubElement(ros2_cs, "ros2").text = get_ros2_version() -ros2_cs_version = ET.SubElement(ros2_cs, "version") -ET.SubElement(ros2_cs_version, "sha").text = get_git_commit(get_ros2cs_path()) -ET.SubElement(ros2_cs_version, "desc").text = get_git_description(get_ros2cs_path()) -ET.SubElement(ros2_cs_version, "date").text = get_commit_date(get_ros2cs_path()) -ET.SubElement(ros2_cs, "standalone").text = str(int(args.standalone)) -rf2u_xmlstr = minidom.parseString(ET.tostring(ros2_for_unity)).toprettyxml(indent=" ") -metadata_rf2u_file = get_ros2_for_unity_path().joinpath("metadata_ros2_for_unity.xml") -with open(str(metadata_rf2u_file), "w") as f: - f.write(rf2u_xmlstr) - -r2cs_xmlstr = minidom.parseString(ET.tostring(ros2_cs)).toprettyxml(indent=" ") -metadata_r2cs_file = get_ros2_for_unity_path().joinpath("metadata_ros2cs.xml") -with open(str(metadata_r2cs_file), "w") as f: - f.write(r2cs_xmlstr) +if __name__ == "__main__": + args = PARSER.parse_args() + metadata = ET.Element("ros2-for-unity") + ET.SubElement(metadata, "ros").text = get_ros2_version() + ET.SubElement(metadata, "standalone").text = "true" if args.standalone else "false" + document = minidom.parseString(ET.tostring(metadata)).toprettyxml() + with open(METADATA_PATH, "w") as file: + file.write(document) diff --git a/usage.png b/usage.png deleted file mode 100644 index b947cb3..0000000 Binary files a/usage.png and /dev/null differ