diff --git a/.gitmodules b/.gitmodules index ec963426..7e8c480a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ -[submodule "ros2_ws/src/Universal_Robots_ROS2_Driver"] - path = ros2_ws/src/Universal_Robots_ROS2_Driver - url = https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git [submodule "engine/o3de"] path = engine/o3de url = https://github.com/o3de/o3de.git diff --git a/Docker/Dockerfile.O3DE b/Docker/Dockerfile.O3DE index 290bcf3f..2de46a24 100644 --- a/Docker/Dockerfile.O3DE +++ b/Docker/Dockerfile.O3DE @@ -4,8 +4,8 @@ # SPDX-License-Identifier: Apache-2.0 OR MIT # -ARG ROS_VERSION=humble -ARG UBUNTU_VERSION=jammy +ARG ROS_VERSION=jazzy +ARG UBUNTU_VERSION=noble FROM ros:${ROS_VERSION}-ros-base-${UBUNTU_VERSION} @@ -13,81 +13,14 @@ ENV WORKSPACE=/data/workspace WORKDIR $WORKSPACE -# O3DE Arguments -ARG O3DE_REPO=https://github.com/o3de/o3de -ARG O3DE_BRANCH=2505.1 -ARG O3DE_COMMIT=HEAD - -# O3DE Extras Arguments -ARG O3DE_EXTRAS_REPO=https://github.com/o3de/o3de-extras -ARG O3DE_EXTRAS_BRANCH=2505.1 -ARG O3DE_EXTRAS_COMMIT=HEAD - -# ROSCon2023 Human Worker Arguments -ARG ROSCON_DEMO_HUMAN_WORKER_REPO=https://github.com/RobotecAI/o3de-humanworker-gem -ARG ROSCON_DEMO_HUMAN_WORKER_BRANCH=2.0.0 -ARG ROSCON_DEMO_HUMAN_WORKER_COMMIT=HEAD - -# ROSCon2023 UR10 and UR20 Robots Gem Arguments -ARG ROSCON_DEMO_UR_ROBOTS_REPO=https://github.com/RobotecAI/o3de-ur-robots-gem -ARG ROSCON_DEMO_UR_ROBOTS_BRANCH=2.0.0 -ARG ROSCON_DEMO_UR_ROBOTS_COMMIT=HEAD - -# ROSCon2023 OTTO 600 and OTTO 1500 Robots Gem Arguments -ARG ROSCON_DEMO_OTTO_ROBOTS_REPO=https://github.com/RobotecAI/o3de-otto-robots-gem -ARG ROSCON_DEMO_OTTO_ROBOTS_BRANCH=2.0.0 -ARG ROSCON_DEMO_OTTO_ROBOTS_COMMIT=HEAD - # ROSConDemo Arguments ARG ROSCON_DEMO_REPO=https://github.com/RobotecAI/ROSCon2023Demo.git -ARG ROSCON_DEMO_BRANCH=3.0.1 +ARG ROSCON_DEMO_BRANCH=main ARG ROSCON_DEMO_COMMIT=HEAD ARG ROSCON_DEMO_LEVEL=level1 ARG ROSCON_DEMO_FULLSCREEN=0 ARG ROSCON_DEMO_LARGE_SCALE=0 -# O3DE Environment -ENV O3DE_REPO=$O3DE_REPO -ENV O3DE_BRANCH=$O3DE_BRANCH -ENV O3DE_COMMIT=$O3DE_COMMIT -ENV O3DE_ROOT=$WORKSPACE/o3de - -# O3DE Extras Environment Variables -ENV O3DE_EXTRAS_REPO=$O3DE_EXTRAS_REPO -ENV O3DE_EXTRAS_BRANCH=$O3DE_EXTRAS_BRANCH -ENV O3DE_EXTRAS_COMMIT=$O3DE_EXTRAS_COMMIT -ENV O3DE_EXTRAS_ROOT=$WORKSPACE/o3de-extras - -# ROSCon2023 Human Worker Environment Variables -ENV ROSCON_DEMO_HUMAN_WORKER_REPO=$ROSCON_DEMO_HUMAN_WORKER_REPO -ENV ROSCON_DEMO_HUMAN_WORKER_BRANCH=$ROSCON_DEMO_HUMAN_WORKER_BRANCH -ENV ROSCON_DEMO_HUMAN_WORKER_COMMIT=$ROSCON_DEMO_HUMAN_WORKER_COMMIT -ENV ROSCON_DEMO_HUMAN_WORKER_ROOT=$WORKSPACE/o3de-humanworker-gem - -# ROSCon2023 UR10 and UR20 Robots Gem Environment Variables -ENV ROSCON_DEMO_UR_ROBOTS_REPO=$ROSCON_DEMO_UR_ROBOTS_REPO -ENV ROSCON_DEMO_UR_ROBOTS_BRANCH=$ROSCON_DEMO_UR_ROBOTS_BRANCH -ENV ROSCON_DEMO_UR_ROBOTS_COMMIT=$ROSCON_DEMO_UR_ROBOTS_COMMIT -ENV ROSCON_DEMO_UR_ROBOTS_ROOT=$WORKSPACE/o3de-ur-robots-gem - -# ROSCon2023 OTTO 600 and OTTO 1500 Robots Gem Environment Variables -ENV ROSCON_DEMO_OTTO_ROBOTS_REPO=$ROSCON_DEMO_OTTO_ROBOTS_REPO -ENV ROSCON_DEMO_OTTO_ROBOTS_BRANCH=$ROSCON_DEMO_OTTO_ROBOTS_BRANCH -ENV ROSCON_DEMO_OTTO_ROBOTS_COMMIT=$ROSCON_DEMO_OTTO_ROBOTS_COMMIT -ENV ROSCON_DEMO_OTTO_ROBOTS_ROOT=$WORKSPACE/o3de-otto-robots-gem - -# Assets only Gems used in the demo -ENV ROSCON_DEMO_WAREHOUSE_ASSETS_REPO=https://github.com/RobotecAI/robotec-warehouse-assets.git -ENV ROSCON_DEMO_WAREHOUSE_ASSETS_ROOT=$WORKSPACE/robotec-warehouse-assets -ENV ROSCON_DEMO_GENERIC_ASSETS_REPO=https://github.com/RobotecAI/robotec-generic-assets.git -ENV ROSCON_DEMO_GENERIC_ASSETS_ROOT=$WORKSPACE/robotec-generic-assets - -# Robotec.ai additional tooling Gems -ENV ROSCON_DEMO_ROBOTEC_GEMS_REPO=https://github.com/RobotecAI/robotec-o3de-tools.git -ENV ROSCON_DEMO_ROBOTEC_GEMS_BRANCH=o3de-2505 -ENV ROSCON_DEMO_ROBOTEC_GEMS_COMMIT=4006ef9 -ENV ROSCON_DEMO_ROBOTEC_GEMS_ROOT=$WORKSPACE/robotec-o3de-tools - # ROSConDemo Environment Variables ENV ROSCON_DEMO_REPO=$ROSCON_DEMO_REPO ENV ROSCON_DEMO_BRANCH=$ROSCON_DEMO_BRANCH @@ -119,11 +52,14 @@ RUN apt-get update \ && apt-get install -y \ binutils \ clang \ + clang-tools \ cmake \ git \ git-lfs \ libglu1-mesa-dev \ libxcb-xinerama0 \ + libxcb-randr0-dev \ + libxcb-keysyms1-dev \ libfontconfig1-dev \ libnvidia-gl-470 \ libxcb-xkb-dev \ @@ -157,10 +93,9 @@ RUN apt-get update \ ros-${ROS_DISTRO}-navigation2 \ ros-${ROS_DISTRO}-ur-description \ ros-${ROS_DISTRO}-force-torque-sensor-broadcaster \ + ros-${ROS_DISTRO}-simulation-interfaces \ python3-vcstool \ - python3-rosdep2 \ - python3-colcon-common-extensions \ - && pip install python-statemachine + python3-colcon-common-extensions ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp diff --git a/Docker/Dockerfile.ROS b/Docker/Dockerfile.ROS index 3e2604a7..eacd62b3 100644 --- a/Docker/Dockerfile.ROS +++ b/Docker/Dockerfile.ROS @@ -4,8 +4,8 @@ # SPDX-License-Identifier: Apache-2.0 OR MIT # -ARG ROS_VERSION=humble -ARG UBUNTU_VERSION=jammy +ARG ROS_VERSION=jazzy +ARG UBUNTU_VERSION=noble FROM ros:${ROS_VERSION}-ros-base-${UBUNTU_VERSION} @@ -64,15 +64,12 @@ RUN apt-get update \ ros-${ROS_DISTRO}-cyclonedds \ ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ ros-${ROS_DISTRO}-controller-manager \ - ros-${ROS_DISTRO}-ur-client-library \ ros-${ROS_DISTRO}-nav2-common \ ros-${ROS_DISTRO}-navigation2 \ - ros-${ROS_DISTRO}-ur-description \ ros-${ROS_DISTRO}-force-torque-sensor-broadcaster \ + ros-${ROS_DISTRO}-simulation-interfaces \ python3-vcstool \ - python3-rosdep2 \ - python3-colcon-common-extensions \ - && pip install python-statemachine + python3-colcon-common-extensions ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp diff --git a/Docker/README.md b/Docker/README.md index 8d986247..c546fa4d 100644 --- a/Docker/README.md +++ b/Docker/README.md @@ -23,28 +23,8 @@ The script for O3DE (`Dockerfile.O3DE`) will build the ROSCon2023Demo Warehouse | Argument | Description | Default | | ------------------------------- | ------------------------------------------------------------------------------------- | --------------------------------------------------- | -| ROS_VERSION | The distro of ROS (humble or iron) | humble | -| UBUNTU_VERSION | The supporting distro of Ubuntu (focal, jammy) | jammy | -| O3DE_REPO | The git repo for O3DE | https://github.com/o3de/o3de | -| O3DE_BRANCH | The branch/tag for O3DE | 2505.1 | -| O3DE_COMMIT | The commit on the branch/tag for O3DE (or HEAD) | HEAD | -| O3DE_EXTRAS_REPO | The git repo for O3DE Extras | https://github.com/o3de/o3de-extras | -| O3DE_EXTRAS_BRANCH | The branch/tag for O3DE Extras | 2505.1 | -| O3DE_EXTRAS_COMMIT | The commit on the branch for O3DE Extras (or HEAD) | HEAD | -| ROSCON_DEMO_HUMAN_WORKER_REPO | The git repo for Demo Human worker Gem | https://github.com/RobotecAI/o3de-humanworker-gem | -| ROSCON_DEMO_HUMAN_WORKER_BRANCH | The branch/tag for Demo Human worker Gem | 2.0.0 | -| ROSCON_DEMO_HUMAN_WORKER_COMMIT | The commit on the branch/tag for Demo Human worker Gem (or HEAD) | HEAD | -| ROSCON_DEMO_UR_ROBOTS_REPO | The git repo for Demo UR Robots Gem | https://github.com/RobotecAI/o3de-ur-robots-gem | -| ROSCON_DEMO_UR_ROBOTS_BRANCH | The branch/tag for Demo UR Robots Gem | 2.0.0 | -| ROSCON_DEMO_UR_ROBOTS_COMMIT | The commit on the branch/tag for Demo UR Robots Gem (or HEAD) | HEAD | -| ROSCON_DEMO_OTTO_ROBOTS_REPO | The git repo for the Demo Otto Robots Gem | https://github.com/RobotecAI/o3de-otto-robots-gem | -| ROSCON_DEMO_OTTO_ROBOTS_BRANCH | The branch/tag for the Demo Otto Robots Gem | 2.0.0 | -| ROSCON_DEMO_OTTO_ROBOTS_COMMIT | The commit on the branch/tag for the Demo Otto Robots Gem (or HEAD) | HEAD | -| ROSCON_DEMO_REPO | The git repo for ROSCon2023 Warehouse Demo | https://github.com/RobotecAI/ROSCon2023Demo.git | -| ROSCON_DEMO_BRANCH | The branch/tag for ROSCon2023 Warehouse Demo | 3.0.1 | -| ROSCON_DEMO_COMMIT | The commit on the branch/tag for ROSCon2023 Warehouse Demo (or HEAD) | HEAD | -| ROSCON_DEMO_ROBOTEC_GEMS_REPO | The git repo for Robotec Gems | https://github.com/RobotecAI/robotec-o3de-tools.git | -| ROSCON_DEMO_ROBOTEC_GEMS_BRANCH | The branch/tag for Robotec Gems | o3de-2505 | +| ROS_VERSION | The distro of ROS (jazzy ) | jazzy | +| UBUNTU_VERSION | The supporting distro of Ubuntu (focal, jammy) | noble | | ROSCON_DEMO_ROBOTEC_GEMS_COMMIT | The commit on the branch/tag for Robotec Gems (or HEAD) | 4006ef9 | | ROSCON_DEMO_LEVEL | The startup level (level1 or level2). **See Notes below** | level1 | | ROSCON_DEMO_FULLSCREEN | Option to launch the simulation in fullscreen mode (0=no, 1=yes) | 0 | @@ -69,7 +49,7 @@ The O3DE Docker image can reach 10 GB in size, so if you want to create a separa | Argument | Description | Default | | ----------------------- | ------------------------------------------------------------------------------------- | ----------------------------------------------- | -| ROS_VERSION | The distro of ROS (humble, or iron) | humble | +| ROS_VERSION | The distro of ROS (humble, or iron) | jazzy | | UBUNTU_VERSION | The supporting distro of Ubuntu (focal, jammy) | jammy | | ROSCON_DEMO_REPO | The git repo for ROSCon2023 Warehouse Demo | https://github.com/RobotecAI/ROSCon2023Demo.git | | ROSCON_DEMO_BRANCH | The branch/tag for ROSCon2023 Warehouse Demo | 2.0.0 | diff --git a/Docker/build_ros.sh b/Docker/build_ros.sh index 0939e843..a74d911e 100755 --- a/Docker/build_ros.sh +++ b/Docker/build_ros.sh @@ -39,9 +39,7 @@ ROSCon2023Demo Project | $(git -C $ROSCON_DEMO_ROOT rev-parse --short # Build and install the additional ROS 2 packages and drivers ############################################################### pushd $ROSCON_DEMO_ROOT/ros2_ws && \ - ./setup_submodules.bash && \ rosdep update && \ - rosdep install --ignore-src --from-paths src/Universal_Robots_ROS2_Driver -y && \ colcon build --symlink-install && \ . ./install/setup.sh && \ popd diff --git a/Docker/build_simulation.sh b/Docker/build_simulation.sh index 81ffbab5..de68a4ad 100755 --- a/Docker/build_simulation.sh +++ b/Docker/build_simulation.sh @@ -21,227 +21,57 @@ fi # Initialize ROS . /opt/ros/${ROS_DISTRO}/setup.sh -############################################################### -# Clone and bootstrap O3DE -############################################################### -echo "Cloning o3de" -git clone --single-branch -b $O3DE_BRANCH $O3DE_REPO $O3DE_ROOT && \ - git -C $O3DE_ROOT lfs install && \ - git -C $O3DE_ROOT lfs pull && \ - git -C $O3DE_ROOT reset --hard $O3DE_COMMIT -if [ $? -ne 0 ] -then - echo "Error cloning o3de from $O3DE_REPO" - exit 1 -fi - -$WORKSPACE/o3de/python/get_python.sh && \ - $WORKSPACE/o3de/scripts/o3de.sh register -ep $O3DE_ROOT -if [ $? -ne 0 ] -then - echo "Error bootstrapping O3DE from $O3DE_REPO" - exit 1 -fi - -############################################################### -# Clone and register o3de-extras -############################################################### -echo "Cloning o3de-extras" -git clone --single-branch -b $O3DE_EXTRAS_BRANCH $O3DE_EXTRAS_REPO $O3DE_EXTRAS_ROOT && \ - git -C $O3DE_EXTRAS_ROOT lfs install && \ - git -C $O3DE_EXTRAS_ROOT lfs pull && \ - git -C $O3DE_EXTRAS_ROOT reset --hard $O3DE_EXTRAS_COMMIT -if [ $? -ne 0 ] -then - echo "Error cloning o3de-extras from $O3DE_EXTRAS_REPO" - exit 1 -fi - -$WORKSPACE/o3de/scripts/o3de.sh register -gp $O3DE_EXTRAS_ROOT/Gems/ROS2 && \ - $WORKSPACE/o3de/scripts/o3de.sh register -gp $O3DE_EXTRAS_ROOT/Gems/ROS2SampleRobots && \ - $WORKSPACE/o3de/scripts/o3de.sh register -gp $O3DE_EXTRAS_ROOT/Gems/WarehouseAssets && \ - $WORKSPACE/o3de/scripts/o3de.sh register -gp $O3DE_EXTRAS_ROOT/Gems/LevelGeoreferencing && \ - $WORKSPACE/o3de/scripts/o3de.sh register -gp $O3DE_EXTRAS_ROOT/Gems/WarehouseAutomation -if [ $? -ne 0 ] -then - echo "Error registering o3de-extras gems" - exit 1 -fi - -############################################################### -# Clone and register the ROSCon2023Demo Human Worker Gem ############################################################### -echo "Cloning the ROSCon2023 Demo Human Worker Gem" -git clone --single-branch -b $ROSCON_DEMO_HUMAN_WORKER_BRANCH $ROSCON_DEMO_HUMAN_WORKER_REPO $ROSCON_DEMO_HUMAN_WORKER_ROOT && \ - git -C $ROSCON_DEMO_HUMAN_WORKER_ROOT lfs install && \ - git -C $ROSCON_DEMO_HUMAN_WORKER_ROOT lfs pull && \ - git -C $ROSCON_DEMO_HUMAN_WORKER_ROOT reset --hard $ROSCON_DEMO_HUMAN_WORKER_COMMIT -if [ $? -ne 0 ] -then - echo "Error cloning ROSCon2023 Demo Human Worker Gem from $ROSCON_DEMO_HUMAN_WORKER_REPO" - exit 1 -fi - -$WORKSPACE/o3de/scripts/o3de.sh register -gp $ROSCON_DEMO_HUMAN_WORKER_ROOT -if [ $? -ne 0 ] -then - echo "Error registering the ROSCon2023 Demo Human Worker Gem" - exit 1 -fi - - -############################################################### -# Clone and register the ROSCon2023Demo UR10 and UR20 Robots Gem +# Clone and register the ROSCon2023Demo ############################################################### -echo "Cloning ROSCon2023 UR10 and UR20 Robots Gem" -git clone --single-branch -b $ROSCON_DEMO_UR_ROBOTS_BRANCH $ROSCON_DEMO_UR_ROBOTS_REPO $ROSCON_DEMO_UR_ROBOTS_ROOT && \ - git -C $ROSCON_DEMO_UR_ROBOTS_ROOT lfs install && \ - git -C $ROSCON_DEMO_UR_ROBOTS_ROOT lfs pull && \ - git -C $ROSCON_DEMO_UR_ROBOTS_ROOT reset --hard $ROSCON_DEMO_UR_ROBOTS_COMMIT -if [ $? -ne 0 ] -then - echo "Error cloning ROSCon2023 UR10 and UR20 Robots Gem from $ROSCON_DEMO_UR_ROBOTS_REPO" - exit 1 -fi - -$WORKSPACE/o3de/scripts/o3de.sh register -gp $ROSCON_DEMO_UR_ROBOTS_ROOT -if [ $? -ne 0 ] -then - echo "Error registering the ROSCon2023 UR10 and UR20 Robots Gem" - exit 1 -fi - -####################################################################### -# Clone and register the ROSCon2023 OTTO 600 and OTTO 1500 Robots Gem -####################################################################### -echo "Cloning ROSCon2023 OTTO 600 and OTTO 1500 Robots Gem" -git clone --single-branch -b $ROSCON_DEMO_OTTO_ROBOTS_BRANCH $ROSCON_DEMO_OTTO_ROBOTS_REPO $ROSCON_DEMO_OTTO_ROBOTS_ROOT && \ - git -C $ROSCON_DEMO_OTTO_ROBOTS_ROOT lfs install && \ - git -C $ROSCON_DEMO_OTTO_ROBOTS_ROOT lfs pull && \ - git -C $ROSCON_DEMO_OTTO_ROBOTS_ROOT reset --hard $ROSCON_DEMO_OTTO_ROBOTS_COMMIT -if [ $? -ne 0 ] -then - echo "Error cloning ROSCon2023 OTTO 600 and OTTO 1500 Robots Gem from $ROSCON_DEMO_OTTO_ROBOTS_REPO" - exit 1 -fi - -$WORKSPACE/o3de/scripts/o3de.sh register -gp $ROSCON_DEMO_OTTO_ROBOTS_ROOT -if [ $? -ne 0 ] -then - echo "Error registering the ROSCon2023 OTTO 600 and OTTO 1500 Robots Gem" - exit 1 -fi - -####################################################################### -# Clone and register assets only gems -####################################################################### -echo "Cloning Warehouse and Generic Assets Gems" -git clone --single-branch $ROSCON_DEMO_WAREHOUSE_ASSETS_REPO $ROSCON_DEMO_WAREHOUSE_ASSETS_ROOT && \ - git -C $ROSCON_DEMO_WAREHOUSE_ASSETS_ROOT lfs install && \ - git -C $ROSCON_DEMO_WAREHOUSE_ASSETS_ROOT lfs pull -if [ $? -ne 0 ] -then - echo "Error cloning Warehouse Assets Gems from $ROSCON_DEMO_WAREHOUSE_ASSETS_REPO" - exit 1 -fi - -$WORKSPACE/o3de/scripts/o3de.sh register -agp $ROSCON_DEMO_WAREHOUSE_ASSETS_ROOT -if [ $? -ne 0 ] -then - echo "Error registering the Warehouse Assets Gems" - exit 1 -fi - -git clone --single-branch $ROSCON_DEMO_GENERIC_ASSETS_REPO $ROSCON_DEMO_GENERIC_ASSETS_ROOT && \ - git -C $ROSCON_DEMO_GENERIC_ASSETS_ROOT lfs install && \ - git -C $ROSCON_DEMO_GENERIC_ASSETS_ROOT lfs pull -if [ $? -ne 0 ] -then - echo "Error cloning Generic Assets Gems from $ROSCON_DEMO_GENERIC_ASSETS_REPO" - exit 1 -fi +git config --global http.postBuffer 524288000 +git config --global http.lowSpeedLimit 0 +git config --global http.lowSpeedTime 300 -$WORKSPACE/o3de/scripts/o3de.sh register -agp $ROSCON_DEMO_GENERIC_ASSETS_ROOT +echo "Cloning the RosCon2023Demo project" +git clone --single-branch -b $ROSCON_DEMO_BRANCH $ROSCON_DEMO_REPO $ROSCON_DEMO_ROOT && \ + git -C $ROSCON_DEMO_ROOT lfs install && \ + git -C $ROSCON_DEMO_ROOT lfs pull && \ + git -C $ROSCON_DEMO_ROOT reset --hard $ROSCON_DEMO_COMMIT if [ $? -ne 0 ] then - echo "Error registering the Generic Assets Gems" + echo "Error cloning RosCon2023Demo project $ROSCON_DEMO_REPO" exit 1 fi -####################################################################### -# Clone and register additional tooling Gems -####################################################################### -echo "Cloning the additional tooling Gems" -git clone --single-branch -b $ROSCON_DEMO_ROBOTEC_GEMS_BRANCH $ROSCON_DEMO_ROBOTEC_GEMS_REPO $ROSCON_DEMO_ROBOTEC_GEMS_ROOT && \ - git -C $ROSCON_DEMO_ROBOTEC_GEMS_ROOT lfs install && \ - git -C $ROSCON_DEMO_ROBOTEC_GEMS_ROOT lfs pull && \ - git -C $ROSCON_DEMO_ROBOTEC_GEMS_ROOT reset --hard $ROSCON_DEMO_ROBOTEC_GEMS_COMMIT +echo "Initializing submodules" +git -C $ROSCON_DEMO_ROOT submodule update --init --recursive if [ $? -ne 0 ] then - echo "Error cloning the additional tooling Gems from $ROSCON_DEMO_ROBOTEC_GEMS_REPO" + echo "Error initializing submodules" exit 1 fi -$WORKSPACE/o3de/scripts/o3de.sh register -gp $ROSCON_DEMO_ROBOTEC_GEMS_ROOT/Gems/ROS2ScriptIntegration +$ROSCON_DEMO_ROOT/engine/o3de/python/get_python.sh if [ $? -ne 0 ] then - echo "Error registering the additional tooling Gems" + echo "Error downloading/configuring O3DE Python" exit 1 fi -############################################################### -# Clone and register the ROSCon2023Demo -############################################################### -echo "Cloning the RosCon2023Demo project" -git clone --single-branch -b $ROSCON_DEMO_BRANCH $ROSCON_DEMO_REPO $ROSCON_DEMO_ROOT && \ - git -C $ROSCON_DEMO_ROOT lfs install && \ - git -C $ROSCON_DEMO_ROOT lfs pull && \ - git -C $ROSCON_DEMO_ROOT reset --hard $ROSCON_DEMO_COMMIT +$ROSCON_DEMO_ROOT/engine/o3de/scripts/o3de.sh register -ep $ROSCON_DEMO_ROOT/engine/o3de if [ $? -ne 0 ] then - echo "Error cloning RosCon2023Demo project $ROSCON_DEMO_REPO" + echo "Error registering the O3DE engine" exit 1 fi -$WORKSPACE/o3de/scripts/o3de.sh register -pp $ROSCON_DEMO_PROJECT -if [ $? -ne 0 ] -then - echo "Error registering the RosCon2023Demo Project" - exit 1 -fi - - -############################################################################### -# Track the git commits from all the repos -############################################################################### -echo -e "\n\ -Repository | Commit | Branch\n\ -----------------------------------+-----------------------------------------\n\ -o3de | $(git -C $O3DE_ROOT rev-parse --short HEAD) | $(git -C $O3DE_ROOT rev-parse --abbrev-ref HEAD) \n\ -o3de-extras | $(git -C $O3DE_EXTRAS_ROOT rev-parse --short HEAD) | $(git -C $O3DE_EXTRAS_ROOT rev-parse --abbrev-ref HEAD) \n\ -ROSCon2023 Demo Human Worker Gem | $(git -C $ROSCON_DEMO_HUMAN_WORKER_ROOT rev-parse --short HEAD) | $(git -C $ROSCON_DEMO_HUMAN_WORKER_ROOT rev-parse --abbrev-ref HEAD) \n\ -ROSCon2023 UR Robots Gem | $(git -C $ROSCON_DEMO_UR_ROBOTS_ROOT rev-parse --short HEAD) | $(git -C $ROSCON_DEMO_UR_ROBOTS_ROOT rev-parse --abbrev-ref HEAD) \n\ -ROSCon2023 OTTO Robots Gem | $(git -C $ROSCON_DEMO_OTTO_ROBOTS_ROOT rev-parse --short HEAD) | $(git -C $ROSCON_DEMO_OTTO_ROBOTS_ROOT rev-parse --abbrev-ref HEAD) \n\ -Warehouse Assets Gems | $(git -C $ROSCON_DEMO_WAREHOUSE_ASSETS_ROOT rev-parse --short HEAD) | $(git -C $ROSCON_DEMO_WAREHOUSE_ASSETS_ROOT rev-parse --abbrev-ref HEAD) \n\ -Generic Assets Gems | $(git -C $ROSCON_DEMO_GENERIC_ASSETS_ROOT rev-parse --short HEAD) | $(git -C $ROSCON_DEMO_GENERIC_ASSETS_ROOT rev-parse --abbrev-ref HEAD) \n\ -ROS2ScriptIntegration Gem | $(git -C $ROSCON_DEMO_ROBOTEC_GEMS_ROOT rev-parse --short HEAD) | $(git -C $ROSCON_DEMO_ROBOTEC_GEMS_ROOT rev-parse --abbrev-ref HEAD) \n\ -RosCon2023Demo Project | $(git -C $ROSCON_DEMO_ROOT rev-parse --short HEAD) | $(git -C $ROSCON_DEMO_ROOT rev-parse --abbrev-ref HEAD) \n\ -\n\ -" >> $WORKSPACE/git_commit.txt - ############################################################### # Set the filename of the level in the registry file ############################################################### -sed -i "s/demolevel1/${DEMO_LEVEL}/g" Registry/load_level.setreg +sed -i "s/demolevel1/${DEMO_LEVEL}/g" $ROSCON_DEMO_PROJECT/Registry/load_level.setreg ############################################################### # Build and install the additional ROS 2 packages and drivers ############################################################### pushd $ROSCON_DEMO_ROOT/ros2_ws && \ - ./setup_submodules.bash && \ rosdep update && \ - rosdep install --ignore-src --from-paths src/Universal_Robots_ROS2_Driver -y && \ colcon build --symlink-install && \ . ./install/setup.sh && \ popd @@ -375,13 +205,13 @@ cp $ROSCON_DEMO_PROJECT/build/game/bin/profile/ROSCon2023Demo.GameLauncher $ROSC cp $ROSCON_DEMO_PROJECT/build/game/bin/profile/*.json $ROSCON_SIMULATION_HOME/ cp $ROSCON_DEMO_PROJECT/build/game/bin/profile/*.so $ROSCON_SIMULATION_HOME -# Cleanup -rm -rf $O3DE_ROOT -rm -rf $O3DE_EXTRAS_ROOT -rm -rf $ROSCON_DEMO_HUMAN_WORKER_ROOT -rm -rf $ROSCON_DEMO_UR_ROBOTS_ROOT -rm -rf $ROSCON_DEMO_OTTO_ROBOTS_ROOT -rm -rf $ROSCON_DEMO_PROJECT +# Cleanup - keep ros2_ws/install for runtime (GameLauncher needs custom message libs) +rm -rf $ROSCON_DEMO_ROOT/engine +rm -rf $ROSCON_DEMO_ROOT/gems +rm -rf $ROSCON_DEMO_ROOT/Project/build +rm -rf $ROSCON_DEMO_ROOT/ros2_ws/src +rm -rf $ROSCON_DEMO_ROOT/ros2_ws/build +rm -rf $ROSCON_DEMO_ROOT/ros2_ws/log rm -rf /root/.o3de exit 0 diff --git a/Project/Assets/BoxMesh/Box5.fbx b/Project/Assets/BoxMesh/Box5.fbx new file mode 100644 index 00000000..10b4fa19 Binary files /dev/null and b/Project/Assets/BoxMesh/Box5.fbx differ diff --git a/Project/Assets/Factory/Box5.prefab b/Project/Assets/Factory/Box5.prefab new file mode 100755 index 00000000..6e50dfde --- /dev/null +++ b/Project/Assets/Factory/Box5.prefab @@ -0,0 +1,332 @@ +{ + "ContainerEntity": { + "Id": "ContainerEntity", + "Name": "Box4", + "Components": { + "EditorDisabledCompositionComponent": { + "$type": "EditorDisabledCompositionComponent", + "Id": 8715409578771130289 + }, + "EditorEntityIconComponent": { + "$type": "EditorEntityIconComponent", + "Id": 8256705382262763300 + }, + "EditorEntitySortComponent": { + "$type": "EditorEntitySortComponent", + "Id": 1541884963070135492, + "Child Entity Order": [ + "Entity_[191891890211650]" + ] + }, + "EditorInspectorComponent": { + "$type": "EditorInspectorComponent", + "Id": 7723945754314690856 + }, + "EditorLockComponent": { + "$type": "EditorLockComponent", + "Id": 12354894372506658654 + }, + "EditorOnlyEntityComponent": { + "$type": "EditorOnlyEntityComponent", + "Id": 9007498808683063333 + }, + "EditorPendingCompositionComponent": { + "$type": "EditorPendingCompositionComponent", + "Id": 14340065101907575651 + }, + "EditorPrefabComponent": { + "$type": "EditorPrefabComponent", + "Id": 3621804511817194472 + }, + "EditorVisibilityComponent": { + "$type": "EditorVisibilityComponent", + "Id": 2622557367775789750 + }, + "TransformComponent": { + "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", + "Id": 4101209608094465705, + "Parent Entity": "" + } + } + }, + "Entities": { + "Entity_[143149087751449]": { + "Id": "Entity_[143149087751449]", + "Name": "VIS", + "Components": { + "AZ::Render::EditorMeshComponent": { + "$type": "AZ::Render::EditorMeshComponent", + "Id": 8675883885105925543, + "Controller": { + "Configuration": { + "ModelAsset": { + "assetId": { + "guid": "{06FD692F-3EFF-513B-9773-57B2E3B5AE6D}", + "subId": 280320852 + }, + "assetHint": "assets/boxmesh/box5.fbx.azmodel" + } + } + } + }, + "EditorDisabledCompositionComponent": { + "$type": "EditorDisabledCompositionComponent", + "Id": 11839446511482333085 + }, + "EditorEntityIconComponent": { + "$type": "EditorEntityIconComponent", + "Id": 14561066232028838835 + }, + "EditorEntitySortComponent": { + "$type": "EditorEntitySortComponent", + "Id": 7030674297684814699 + }, + "EditorInspectorComponent": { + "$type": "EditorInspectorComponent", + "Id": 13226171051440185542, + "ComponentOrderEntryArray": [ + { + "ComponentId": 8903659711004105110 + }, + { + "ComponentId": 13569015630530104047, + "SortIndex": 1 + }, + { + "ComponentId": 13226171051440185542, + "SortIndex": 2 + }, + { + "ComponentId": 7030674297684814699, + "SortIndex": 3 + }, + { + "ComponentId": 12358938230356911200, + "SortIndex": 4 + }, + { + "ComponentId": 4848749736236174155, + "SortIndex": 5 + }, + { + "ComponentId": 554919628147339253, + "SortIndex": 6 + }, + { + "ComponentId": 14561066232028838835, + "SortIndex": 7 + }, + { + "ComponentId": 11839446511482333085, + "SortIndex": 8 + }, + { + "ComponentId": 8675883885105925543, + "SortIndex": 9 + }, + { + "ComponentId": 7097559180465575381, + "SortIndex": 10 + } + ] + }, + "EditorLockComponent": { + "$type": "EditorLockComponent", + "Id": 4848749736236174155 + }, + "EditorMaterialComponent": { + "$type": "EditorMaterialComponent", + "Id": 7097559180465575381, + "Controller": { + "Configuration": { + "materials": [ + { + "Key": { + "materialSlotStableId": 1773863654 + }, + "Value": { + "MaterialAsset": { + "assetId": { + "guid": "{5468360F-1789-50A3-96FF-118192B81B48}" + }, + "assetHint": "assets/warehouse/materials/mwarehouseboxes.azmaterial" + } + } + }, + { + "Key": { + "materialSlotStableId": 2919702015 + }, + "Value": { + "MaterialAsset": { + "assetId": { + "guid": "{5468360F-1789-50A3-96FF-118192B81B48}" + }, + "assetHint": "assets/warehouse/materials/mwarehouseboxes.azmaterial" + } + } + } + ] + } + } + }, + "EditorOnlyEntityComponent": { + "$type": "EditorOnlyEntityComponent", + "Id": 13569015630530104047 + }, + "EditorPendingCompositionComponent": { + "$type": "EditorPendingCompositionComponent", + "Id": 554919628147339253 + }, + "EditorVisibilityComponent": { + "$type": "EditorVisibilityComponent", + "Id": 12358938230356911200 + }, + "TransformComponent": { + "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", + "Id": 8903659711004105110, + "Parent Entity": "Entity_[191891890211650]", + "Transform Data": { + "Translate": [ + 0.0, + 0.0, + -0.10000000149011612 + ] + } + } + } + }, + "Entity_[191891890211650]": { + "Id": "Entity_[191891890211650]", + "Name": "Box5", + "Components": { + "Component_[13574867754902005040]": { + "$type": "EditorInspectorComponent", + "Id": 13574867754902005040, + "ComponentOrderEntryArray": [ + { + "ComponentId": 16246303725359178974 + }, + { + "ComponentId": 5935093852243013826, + "SortIndex": 1 + }, + { + "ComponentId": 7605271439870430818, + "SortIndex": 2 + }, + { + "ComponentId": 3144205669266579244, + "SortIndex": 3 + }, + { + "ComponentId": 1552691188429355007, + "SortIndex": 4 + }, + { + "ComponentId": 17867042698575286426, + "SortIndex": 5 + } + ] + }, + "Component_[13890549230067286886]": { + "$type": "EditorOnlyEntityComponent", + "Id": 13890549230067286886 + }, + "Component_[14299065573721311548]": { + "$type": "EditorVisibilityComponent", + "Id": 14299065573721311548 + }, + "Component_[16246303725359178974]": { + "$type": "{27F1E1A1-8D9D-4C3B-BD3A-AFB9762449C0} TransformComponent", + "Id": 16246303725359178974, + "Parent Entity": "ContainerEntity" + }, + "Component_[17438889026096965443]": { + "$type": "EditorDisabledCompositionComponent", + "Id": 17438889026096965443 + }, + "Component_[2635600976954600347]": { + "$type": "EditorEntitySortComponent", + "Id": 2635600976954600347, + "Child Entity Order": [ + "Entity_[143149087751449]" + ] + }, + "Component_[7619205353932880281]": { + "$type": "EditorLockComponent", + "Id": 7619205353932880281 + }, + "Component_[8458290980975423191]": { + "$type": "EditorEntityIconComponent", + "Id": 8458290980975423191 + }, + "Component_[9483887954899180333]": { + "$type": "EditorPendingCompositionComponent", + "Id": 9483887954899180333 + }, + "EditorColliderComponent": { + "$type": "EditorColliderComponent", + "Id": 17867042698575286426, + "ColliderConfiguration": { + "MaterialSlots": { + "Slots": [ + { + "Name": "Entire object", + "MaterialAsset": { + "assetId": { + "guid": "{7FC39784-D1D8-5D99-B4B3-1EB59CF8EAF6}", + "subId": 1 + }, + "assetHint": "factory/conveyorline/box.physicsmaterial" + } + } + ] + } + }, + "ShapeConfiguration": { + "ShapeType": 1, + "Box": { + "Configuration": [ + 0.30000001192092896, + 0.30000001192092896, + 0.20000000298023224 + ] + } + }, + "DebugDrawSettings": { + "LocallyEnabled": false + } + }, + "EditorRigidBodyComponent": { + "$type": "EditorRigidBodyComponent", + "Id": 1552691188429355007, + "Configuration": { + "entityId": "", + "Compute Mass": false, + "Mass": 0.5, + "Inertia tensor": [ + 0.005416667088866234, + 0.0, + 0.0, + 0.0, + 0.005416667088866234, + 0.0, + 0.0, + 0.0, + 0.0075000012293457985 + ] + } + }, + "EditorTagComponent": { + "$type": "EditorTagComponent", + "Id": 17200051048558680531, + "Tags": [ + "Grippable", + "Box" + ] + } + } + } + } +} \ No newline at end of file diff --git a/Project/Assets/Factory/Objects/PalletWithBarrell.prefab b/Project/Assets/Factory/Objects/PalletWithBarrell.prefab index ea11dad3..bf2902e2 100755 --- a/Project/Assets/Factory/Objects/PalletWithBarrell.prefab +++ b/Project/Assets/Factory/Objects/PalletWithBarrell.prefab @@ -190,7 +190,7 @@ ] }, "Instance_[985462854478322]": { - "Source": "PlasticBarrel/PlasticBarrel.prefab", + "Source": "PlasticBarrel/PlasticBarrel1.prefab", "Patches": [ { "op": "replace", diff --git a/Project/Assets/Factory/Signs/WarningPlates_Restricted.fbx b/Project/Assets/Factory/Signs/WarningPlates_Restricted.fbx index d5901260..b745d96f 100644 Binary files a/Project/Assets/Factory/Signs/WarningPlates_Restricted.fbx and b/Project/Assets/Factory/Signs/WarningPlates_Restricted.fbx differ diff --git a/Project/Gem/CMakeLists.txt b/Project/Gem/CMakeLists.txt index 468f5101..8ebe5ee3 100644 --- a/Project/Gem/CMakeLists.txt +++ b/Project/Gem/CMakeLists.txt @@ -63,6 +63,11 @@ ly_add_target( AZ::AzGameFramework Gem::Atom_AtomBridge.Static Gem::ROS2.Static + Gem::ROS2Sensors.API + Gem::ImGui.Static + Gem::ImGui.imguilib + Gem::LmbrCentral.API + Gem::PhysX5.Static Gem::RecastNavigation.API Gem::EMotionFX.Static ) @@ -84,6 +89,9 @@ ly_add_target( AZ::AzCore Gem::RecastNavigation.API Gem::ROS2.Static + Gem::ROS2Sensors.API + Gem::ImGui.imguilib + Gem::LmbrCentral.API ) find_package(OpenCV REQUIRED) diff --git a/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.cpp b/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.cpp index 17b03ccc..8a38f78f 100644 --- a/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.cpp +++ b/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.cpp @@ -16,12 +16,12 @@ namespace ROS2::Demo void GrayscaleCamera::Activate() { AZ_Printf("GrayscaleCamera", "GrayscaleCamera activated"); - CameraPostProcessingRequestBus::Handler::BusConnect(GetEntityId()); + ROS2Sensors::CameraPostProcessingRequestBus::Handler::BusConnect(GetEntityId()); } void GrayscaleCamera::Deactivate() { - CameraPostProcessingRequestBus::Handler::BusDisconnect(); + ROS2Sensors::CameraPostProcessingRequestBus::Handler::BusDisconnect(); } void GrayscaleCamera::Reflect(AZ::ReflectContext* context) diff --git a/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.h b/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.h index 5fd51266..9c4a68a3 100644 --- a/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.h +++ b/Project/Gem/Source/CameraPostprocess/GrayscaleCamera.h @@ -10,14 +10,13 @@ #include #include -#include +#include namespace ROS2::Demo { - class GrayscaleCamera : public AZ::Component - , private CameraPostProcessingRequestBus::Handler + , private ROS2Sensors::CameraPostProcessingRequestBus::Handler { public: AZ_COMPONENT(GrayscaleCamera, "{0191e0df-a2df-748a-9ddf-6e1337d8cb73}"); diff --git a/Project/Gem/Source/Scripting/FoilWrapperController.cpp b/Project/Gem/Source/Scripting/FoilWrapperController.cpp index b8af0038..6871da17 100644 --- a/Project/Gem/Source/Scripting/FoilWrapperController.cpp +++ b/Project/Gem/Source/Scripting/FoilWrapperController.cpp @@ -7,6 +7,8 @@ #include #include #include +#include + namespace ROS2::Demo { void FoilWrapperConfig::Reflect(AZ::ReflectContext* context) @@ -55,6 +57,7 @@ namespace ROS2::Demo void FoilWrapper::Activate() { + m_onTriggerEnterHandler = AzPhysics::SimulatedBodyEvents::OnTriggerEnter::Handler( [&]([[maybe_unused]] AzPhysics::SimulatedBodyHandle bodyHandle, [[maybe_unused]] const AzPhysics::TriggerEvent& event) { @@ -88,10 +91,13 @@ namespace ROS2::Demo }); AZ::TickBus::Handler::BusConnect(); + ImGui::ImGuiUpdateListenerBus::Handler::BusConnect(); + } void FoilWrapper::Deactivate() { + ImGui::ImGuiUpdateListenerBus::Handler::BusDisconnect(); AZ::TickBus::Handler::BusDisconnect(); } @@ -191,10 +197,9 @@ namespace ROS2::Demo m_configuration.m_foilWrapperEntityId, &EMotionFX::Integration::SimpleMotionComponentRequestBus::Events::LoopMotion, false); - EMotionFX::Integration::SimpleMotionComponentRequestBus::Event( m_configuration.m_foilWrapperEntityId, &EMotionFX::Integration::SimpleMotionComponentRequestBus::Events::PlayMotion); - + AZ_Printf("FoilWrapper", "Starting to wrap the pallet, waiting for the animation to start..."); m_state = FoilWrapperState::Wrapping; m_timer = 0; } @@ -270,4 +275,24 @@ namespace ROS2::Demo } } } + + void FoilWrapper::OnImGuiUpdate() + { + AZStd::string label = AZStd::string::format("FoilWrapperDebugger %s", m_entity->GetName().c_str()); + + ImGui::Begin(label.c_str()); + + if (ImGui::Button("FoilWrapper Animation")) + { + EMotionFX::Integration::SimpleMotionComponentRequestBus::Event( + m_configuration.m_foilWrapperEntityId, + &EMotionFX::Integration::SimpleMotionComponentRequestBus::Events::LoopMotion, + false); + EMotionFX::Integration::SimpleMotionComponentRequestBus::Event( + m_configuration.m_foilWrapperEntityId, &EMotionFX::Integration::SimpleMotionComponentRequestBus::Events::PlayMotion); + } + ImGui::End(); + } + + } // namespace ROS2::Demo diff --git a/Project/Gem/Source/Scripting/FoilWrapperController.h b/Project/Gem/Source/Scripting/FoilWrapperController.h index 1cc5fc4b..150ab7c3 100644 --- a/Project/Gem/Source/Scripting/FoilWrapperController.h +++ b/Project/Gem/Source/Scripting/FoilWrapperController.h @@ -20,6 +20,7 @@ #include #include #include +#include namespace ROS2::Demo { @@ -44,6 +45,7 @@ namespace ROS2::Demo class FoilWrapper : public AZ::Component , public AZ::TickBus::Handler + , public ImGui::ImGuiUpdateListenerBus::Handler { enum class FoilWrapperState { @@ -70,6 +72,9 @@ namespace ROS2::Demo static void Reflect(AZ::ReflectContext* context); private: + // ImGui::ImGuiUpdateListenerBus::Handler overrides... + void OnImGuiUpdate() override; + FoilWrapperConfig m_configuration; FoilWrapperState m_state = FoilWrapperState::Idle; float m_timer = 0.0f; diff --git a/Project/Gem/Source/Scripting/ObjectDetectionComponent.cpp b/Project/Gem/Source/Scripting/ObjectDetectionComponent.cpp index 7b1b682f..60ac004b 100644 --- a/Project/Gem/Source/Scripting/ObjectDetectionComponent.cpp +++ b/Project/Gem/Source/Scripting/ObjectDetectionComponent.cpp @@ -21,7 +21,6 @@ #include #include #include -#include #include namespace ROS2::Demo @@ -76,9 +75,12 @@ namespace ROS2::Demo AZ_Assert(ROS2Interface, "ROS2Interface not available"); auto ROS2Node = ROS2Interface->GetNode(); AZ_Assert(ROS2Node, "ROS2Node not available"); - auto ros2Frame = ROS2::Utils::GetGameOrEditorComponent(GetEntity()); - AZ_Assert(ros2Frame, "Missing ROS2FrameComponent"); - AZStd::string topicName = ros2Frame->GetNamespace() + m_configuration.m_topicConfiguration.m_topic; + AZStd::string namespaceFromFrame; + ROS2::ROS2FrameComponentBus::EventResult(namespaceFromFrame, m_entity->GetId(), &ROS2::ROS2FrameComponentRequests::GetNamespace); + + AZStd::string topicName; + ROS2::ROS2NamesRequestBus::BroadcastResult(topicName, &ROS2::ROS2NamesRequestBus::Events::GetNamespacedName, namespaceFromFrame, m_configuration.m_topicConfiguration.m_topic); + m_topicPublisher = ROS2Node->create_publisher(topicName.c_str(), m_configuration.m_topicConfiguration.GetQoS()); diff --git a/Project/Gem/Source/Scripting/ScriptSpawnLevelComponent.cpp b/Project/Gem/Source/Scripting/ScriptSpawnLevelComponent.cpp index 18c8b9b4..06317b5e 100644 --- a/Project/Gem/Source/Scripting/ScriptSpawnLevelComponent.cpp +++ b/Project/Gem/Source/Scripting/ScriptSpawnLevelComponent.cpp @@ -93,7 +93,7 @@ namespace ROS2::Demo AzFramework::SpawnAllEntitiesOptionalArgs optionalArgs; AzFramework::EntitySpawnTicket ticket(spawnable); // Set the pre-spawn callback to set the name of the root entity to the name of the spawnable - optionalArgs.m_preInsertionCallback = [transform, spawnableName, parent](auto id, auto view) + optionalArgs.m_preInsertionCallback = [transform, spawnableName](auto id, auto view) { if (view.empty()) { @@ -109,16 +109,11 @@ namespace ROS2::Demo AZ_Assert(childEntity, "Invalid child entity"); childEntity->SetName(spawnableName.c_str()); } - auto* transformInterface = root->FindComponent(); transformInterface->SetWorldTM(transform); - if (parent.IsValid()) - { - transformInterface->SetParent(parent); - } }; - optionalArgs.m_completionCallback = [this, spawnableName](auto ticket, auto result) + optionalArgs.m_completionCallback = [this, spawnableName, parent](auto ticket, auto result) { if (!result.empty() && result.size() > 1) { @@ -126,6 +121,15 @@ namespace ROS2::Demo const AZ::Entity* firstChild = *(result.begin() + 1); m_spawnedEntitiesToNames[spawnableName] = firstChild->GetId(); } + + if (parent.IsValid() && !result.empty()) + { + const AZ::Entity* child = *result.begin(); + AZ_Assert(child, "Invalid child entity"); + auto* transformInterface = child->FindComponent(); + transformInterface->SetParent(parent); + } + }; optionalArgs.m_priority = AzFramework::SpawnablePriority_Lowest; diff --git a/Project/Gem/Source/Vision/IdealVisionSystem.cpp b/Project/Gem/Source/Vision/IdealVisionSystem.cpp index 2aa1edfd..4e84a734 100644 --- a/Project/Gem/Source/Vision/IdealVisionSystem.cpp +++ b/Project/Gem/Source/Vision/IdealVisionSystem.cpp @@ -22,9 +22,10 @@ #include #include #include +#include #include -#include - +#include +#include namespace ROS2::Demo { @@ -123,9 +124,13 @@ namespace ROS2::Demo void IdealVisionSystem::Activate() { - ROS2::CameraCalibrationRequestBus::EventResult(m_cameraMatrix, GetEntityId(), &ROS2::CameraCalibrationRequest::GetCameraMatrix); - ROS2::CameraCalibrationRequestBus::EventResult(m_cameraWidth, GetEntityId(), &ROS2::CameraCalibrationRequest::GetWidth); - ROS2::CameraCalibrationRequestBus::EventResult(m_cameraHeight, GetEntityId(), &ROS2::CameraCalibrationRequest::GetHeight); + ROS2Sensors::CameraConfigurationRequestBus::EventResult(m_cameraMatrix, GetEntityId(), &ROS2Sensors::CameraConfigurationRequest::GetCameraMatrix); + int cameraWidth = 0; + ROS2Sensors::CameraConfigurationRequestBus::EventResult(cameraWidth, GetEntityId(), &ROS2Sensors::CameraConfigurationRequest::GetWidth); + m_cameraWidth = static_cast(cameraWidth); + int cameraHeight = 0; + ROS2Sensors::CameraConfigurationRequestBus::EventResult(cameraHeight, GetEntityId(), &ROS2Sensors::CameraConfigurationRequest::GetHeight); + m_cameraHeight = static_cast(cameraHeight); m_frustumPoints = CreateFrustumPoints(m_cameraMatrix, m_cameraWidth, m_cameraHeight, m_configuration.m_maximumDetectionRange); m_cookedMesh = CookFrustumMesh(m_frustumPoints); @@ -141,17 +146,18 @@ namespace ROS2::Demo m_sensorConfiguration.m_publishersConfigurations.size() == 3, "Invalid configuration of publishers for IdealVisionSystem sensor"); const TopicConfiguration& publisherConfig2DDetection = m_sensorConfiguration.m_publishersConfigurations[Detection2DType]; - AZStd::string fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig2DDetection.m_topic); + AZStd::string fullTopic; + ROS2::ROS2NamesRequestBus::BroadcastResult(fullTopic, &ROS2::ROS2NamesRequests::GetNamespacedName, GetNamespace(), publisherConfig2DDetection.m_topic); m_detection2DPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig2DDetection.GetQoS()); const TopicConfiguration& publisherConfig3DDetection = m_sensorConfiguration.m_publishersConfigurations[Detection3DType]; - fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfig3DDetection.m_topic); + ROS2::ROS2NamesRequestBus::BroadcastResult(fullTopic, &ROS2::ROS2NamesRequests::GetNamespacedName, GetNamespace(), publisherConfig3DDetection.m_topic); m_detection3DPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfig3DDetection.GetQoS()); const TopicConfiguration& publisherConfigPoseArray = m_sensorConfiguration.m_publishersConfigurations[DetectionPoseArrayType]; - fullTopic = ROS2Names::GetNamespacedName(GetNamespace(), publisherConfigPoseArray.m_topic); + ROS2::ROS2NamesRequestBus::BroadcastResult(fullTopic, &ROS2::ROS2NamesRequests::GetNamespacedName, GetNamespace(), publisherConfigPoseArray.m_topic); m_detectionArrayPublisher = ros2Node->create_publisher(fullTopic.data(), publisherConfigPoseArray.GetQoS()); @@ -242,8 +248,13 @@ namespace ROS2::Demo vision_msgs::msg::Detection3DArray detection3DArray; std_msgs::msg::Header header; - header.frame_id = GetFrameID().c_str(); - header.stamp = ROS2Interface::Get()->GetROSTimestamp(); + + + AZStd::string frameName; + ROS2FrameComponentBus::EventResult(frameName, m_entity->GetId(), &ROS2::ROS2FrameComponentRequests::GetNamespacedFrameID); + + header.frame_id = frameName.c_str(); + header.stamp = ROS2::ROS2ClockInterface::Get()->GetROSTimestamp(); poseArray.header = header; detection3DArray.header = header; @@ -275,18 +286,8 @@ namespace ROS2::Demo AZ::ComponentApplicationBus::BroadcastResult( targetName, &AZ::ComponentApplicationBus::Events::GetEntityName, result.m_entityId); - AZ::Entity* targetEntity = nullptr; - AZ::ComponentApplicationBus::BroadcastResult( - targetEntity, &AZ::ComponentApplicationBus::Events::FindEntity, result.m_entityId); - - AZ_Assert(targetEntity, "IdealVisionSystem result entity not found"); - - auto targetROS2FrameComponent = ROS2::Utils::GetGameOrEditorComponent(targetEntity); AZStd::string frameNamespace = ""; - if (targetROS2FrameComponent) - { - frameNamespace = targetROS2FrameComponent->GetNamespace(); - } + ROS2::ROS2FrameComponentBus::EventResult(frameNamespace, result.m_entityId, &ROS2::ROS2FrameComponentRequests::GetNamespace); AZStd::string targetId = frameNamespace + "/" + targetName; diff --git a/Project/Gem/Source/Vision/IdealVisionSystem.h b/Project/Gem/Source/Vision/IdealVisionSystem.h index 005a6ec3..49bf51ac 100644 --- a/Project/Gem/Source/Vision/IdealVisionSystem.h +++ b/Project/Gem/Source/Vision/IdealVisionSystem.h @@ -15,7 +15,6 @@ #include #include #include -#include #include #include #include diff --git a/Project/Levels/DemoLevel1/DemoLevel1.prefab b/Project/Levels/DemoLevel1/DemoLevel1.prefab index 90af2826..e40365d9 100644 --- a/Project/Levels/DemoLevel1/DemoLevel1.prefab +++ b/Project/Levels/DemoLevel1/DemoLevel1.prefab @@ -30,7 +30,8 @@ "Entity_[157997884966234]", "Entity_[1842731393655449]", "Instance_[447351292908083]/ContainerEntity", - "Instance_[46481451049499]/ContainerEntity" + "Instance_[46481451049499]/ContainerEntity", + "Instance_[33076730978615]/ContainerEntity" ] }, "Component_[15230859088967841193]": { @@ -938,10 +939,10 @@ "configuration": { "Spawnable": { "assetId": { - "guid": "{A340D2D4-3D29-5C22-ADC1-FAAF9C2CE30B}", - "subId": 1877876294 + "guid": "{C420DDAB-BCD2-5826-9D8B-45204E56B5CE}", + "subId": 2124516451 }, - "assetHint": "assets/factory/box2.spawnable" + "assetHint": "assets/factory/box5.spawnable" }, "MaxBoxCount": 4, "BoxSpawnInterval": 3.0, @@ -1354,10 +1355,10 @@ "configuration": { "Spawnable": { "assetId": { - "guid": "{A340D2D4-3D29-5C22-ADC1-FAAF9C2CE30B}", - "subId": 1877876294 + "guid": "{C420DDAB-BCD2-5826-9D8B-45204E56B5CE}", + "subId": 2124516451 }, - "assetHint": "assets/factory/box2.spawnable" + "assetHint": "assets/factory/box5.spawnable" }, "MaxBoxCount": 4, "BoxSpawnInterval": 3.0, @@ -1982,10 +1983,10 @@ "configuration": { "Spawnable": { "assetId": { - "guid": "{A340D2D4-3D29-5C22-ADC1-FAAF9C2CE30B}", - "subId": 1877876294 + "guid": "{C420DDAB-BCD2-5826-9D8B-45204E56B5CE}", + "subId": 2124516451 }, - "assetHint": "assets/factory/box2.spawnable" + "assetHint": "assets/factory/box5.spawnable" }, "MaxBoxCount": 4, "BoxSpawnInterval": 3.0, @@ -2166,10 +2167,10 @@ "configuration": { "Spawnable": { "assetId": { - "guid": "{A340D2D4-3D29-5C22-ADC1-FAAF9C2CE30B}", - "subId": 1877876294 + "guid": "{C420DDAB-BCD2-5826-9D8B-45204E56B5CE}", + "subId": 2124516451 }, - "assetHint": "assets/factory/box2.spawnable" + "assetHint": "assets/factory/box5.spawnable" }, "MaxBoxCount": 4, "BoxSpawnInterval": 3.0, @@ -2504,6 +2505,31 @@ } ] }, + "Instance_[33076730978615]": { + "Source": "Assets/Factory/Box5.prefab", + "Patches": [ + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", + "value": "../Entity_[1146574390643]" + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", + "value": -1.976193904876709 + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", + "value": 10.68496322631836 + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/2", + "value": 7.803696632385254 + } + ] + }, "Instance_[4001389725264]": { "Source": "Assets/Factory/Workstation/CellConveyor.prefab", "Patches": [ @@ -2528,199 +2554,269 @@ "Source": "Assets/Factory/Objects/WarehouseObjects_300m2.prefab", "Patches": [ { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/1", - "value": "Instance_[612481889784870]/ContainerEntity" + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", + "value": "../Entity_[1146574390643]" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/2", - "value": "Instance_[852429577377864]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/18", + "value": "Instance_[547916396091464]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/3", - "value": "Instance_[553267175674918]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/19", + "value": "Instance_[547929280993352]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/4", - "value": "Instance_[731864800744486]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/20", + "value": "Instance_[399263283009608]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/5", - "value": "Instance_[437298763715622]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/21", + "value": "Instance_[399250398107720]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/6", - "value": "Instance_[437272993911846]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/22", + "value": "Instance_[351481771841608]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/7", - "value": "Instance_[437247224108070]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/23", + "value": "Instance_[351468886939720]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/8", - "value": "Instance_[437260109009958]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/24", + "value": "Instance_[399276167911496]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/9", - "value": "Instance_[852399512606792]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/25", + "value": "Instance_[399211743402056]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/10", - "value": "Instance_[612507659588646]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/26", + "value": "Instance_[547877741385800]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/11", - "value": "Instance_[553237110903846]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/27", + "value": "Instance_[323941691872294]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/12", - "value": "Instance_[672117510689830]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/28", + "value": "Instance_[448457838417992]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/13", - "value": "Instance_[553207046132774]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/29", + "value": "Instance_[399224628303944]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/14", - "value": "Instance_[612494774686758]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/30", + "value": "Instance_[698888791513160]/ContainerEntity" }, { - "op": "add", - "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/15", - "value": "Instance_[437285878813734]/ContainerEntity" + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/31", + "value": "Instance_[598107383912520]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/32", + "value": "Instance_[323928806970406]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/33", + "value": "Instance_[399173088696392]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/34", + "value": "Instance_[399185973598280]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/35", + "value": "Instance_[448483608221768]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/36", + "value": "Instance_[448470723319880]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/37", + "value": "Instance_[323915922068518]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/38", + "value": "Instance_[399198858500168]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/39", + "value": "Instance_[351456002037832]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/40", + "value": "Instance_[399160203794504]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/41", + "value": "Instance_[399237513205832]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/42", + "value": "Instance_[397537968252480]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/43", + "value": "Instance_[448483608221768]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/44", + "value": "Instance_[448470723319880]/ContainerEntity" + }, + { + "op": "replace", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/45", + "value": "Instance_[399276167911496]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/18", - "value": "Instance_[547916396091464]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/46", + "value": "Instance_[399160203794504]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/19", - "value": "Instance_[547929280993352]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/47", + "value": "Instance_[448457838417992]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/20", - "value": "Instance_[399263283009608]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/48", + "value": "Instance_[320282437192559]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/21", - "value": "Instance_[399250398107720]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/49", + "value": "Instance_[698888791513160]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/22", - "value": "Instance_[351481771841608]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/50", + "value": "Instance_[399224628303944]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/23", - "value": "Instance_[351468886939720]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/51", + "value": "Instance_[598107383912520]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/24", - "value": "Instance_[399276167911496]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/52", + "value": "Instance_[399198858500168]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/25", - "value": "Instance_[399211743402056]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/53", + "value": "Instance_[547916396091464]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/26", - "value": "Instance_[547877741385800]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/54", + "value": "Instance_[399173088696392]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/27", - "value": "Instance_[323941691872294]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/55", + "value": "Instance_[547877741385800]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/28", - "value": "Instance_[448457838417992]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/56", + "value": "Instance_[351468886939720]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/29", - "value": "Instance_[399224628303944]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/57", + "value": "Instance_[320295322094447]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/30", - "value": "Instance_[698888791513160]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/58", + "value": "Instance_[323915922068518]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/31", - "value": "Instance_[598107383912520]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/59", + "value": "Instance_[399250398107720]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/32", - "value": "Instance_[323928806970406]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/60", + "value": "Instance_[351481771841608]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/33", - "value": "Instance_[399173088696392]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/61", + "value": "Instance_[323941691872294]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/34", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/62", "value": "Instance_[399185973598280]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/35", - "value": "Instance_[448483608221768]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/63", + "value": "Instance_[399211743402056]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/36", - "value": "Instance_[448470723319880]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/64", + "value": "Instance_[399237513205832]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/37", - "value": "Instance_[323915922068518]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/65", + "value": "Instance_[323928806970406]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/38", - "value": "Instance_[399198858500168]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/66", + "value": "Instance_[547929280993352]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/39", + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/67", "value": "Instance_[351456002037832]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/40", - "value": "Instance_[399160203794504]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/68", + "value": "Instance_[399263283009608]/ContainerEntity" }, { "op": "add", - "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/41", - "value": "Instance_[399237513205832]/ContainerEntity" + "path": "/Entities/Entity_[351417347332168]/Components/EditorEntitySortComponent/Child Entity Order/69", + "value": "Instance_[53661239531787]/ContainerEntity" }, { "op": "add", @@ -2740,7 +2836,7 @@ { "op": "add", "path": "/Entities/Entity_[351430232234056]/Components/EditorEntitySortComponent/Child Entity Order/13", - "value": "Instance_[498111955327048]/ContainerEntity" + "value": "" }, { "op": "add", @@ -2757,11 +2853,94 @@ "path": "/Entities/Entity_[372520970666328]/Components/EditorEntitySortComponent/Child Entity Order/26", "value": "Instance_[380064029529126]/ContainerEntity" }, + { + "op": "remove", + "path": "/Entities/Entity_[372529560600920]/Components/EditorEntitySortComponent/Child Entity Order/5" + }, + { + "op": "remove", + "path": "/Entities/Entity_[372598280077656]/Components/EditorEntitySortComponent/Child Entity Order/7" + }, { "op": "add", "path": "/Entities/Entity_[372606870012248]/Components/EditorEntitySortComponent/Child Entity Order/25", "value": "Instance_[792213386220582]/ContainerEntity" }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/1", + "value": "Instance_[612481889784870]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/2", + "value": "Instance_[852429577377864]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/3", + "value": "Instance_[553267175674918]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/4", + "value": "Instance_[731864800744486]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/5", + "value": "Instance_[437298763715622]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/6", + "value": "Instance_[437272993911846]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/7", + "value": "Instance_[437247224108070]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/8", + "value": "Instance_[437260109009958]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/9", + "value": "Instance_[852399512606792]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/10", + "value": "Instance_[612507659588646]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/11", + "value": "Instance_[553237110903846]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/12", + "value": "Instance_[672117510689830]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/13", + "value": "Instance_[553207046132774]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/14", + "value": "Instance_[612494774686758]/ContainerEntity" + }, + { + "op": "add", + "path": "/Entities/Entity_[800262904600648]/Components/EditorEntitySortComponent/Child Entity Order/15", + "value": "Instance_[437285878813734]/ContainerEntity" + }, { "op": "add", "path": "/Entities/Entity_[904463106168904]/Components/EditorEntitySortComponent/Child Entity Order/4", @@ -2810,7 +2989,7 @@ { "op": "add", "path": "/Entities/Entity_[904463106168904]/Components/EditorEntitySortComponent/Child Entity Order/13", - "value": "Instance_[904600545122376]/ContainerEntity" + "value": "" }, { "op": "add", @@ -2842,10 +3021,203 @@ "path": "/Entities/Entity_[904463106168904]/Components/EditorEntitySortComponent/Child Entity Order/19", "value": "Instance_[380016784888870]/ContainerEntity" }, + { + "op": "remove", + "path": "/Instances/Instance_[102588833340186]" + }, + { + "op": "remove", + "path": "/Instances/Instance_[102597423274778]" + }, { "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[1146574390643]" + "path": "/Instances/Instance_[233886234173254]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233886234173254]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233881939205958]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233881939205958]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233877644238662]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233877644238662]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233864759336774]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233864759336774]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233813219729222]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233813219729222]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233808924761926]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233808924761926]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233632831102790]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[233632831102790]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[1549978296134]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[1549978296134]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[102601718242074]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[102601718242074]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[102593128307482]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Asset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[102593128307482]/Entities/Entity_[768075193863]/Components/EditorMeshColliderComponent/ShapeConfiguration/PhysicsAsset/Configuration/PhysicsAsset/assetHint", + "value": "otto1500/models/otto1500_standlow.fbx.pxmesh" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14801240651808]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14801240651808]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14801240651808]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14788355749920]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14788355749920]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14788355749920]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14775470848032]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14775470848032]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14775470848032]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14762585946144]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14762585946144]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14762585946144]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14749701044256]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14749701044256]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14749701044256]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14728226207776]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14728226207776]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[29443303357176]/Instances/Instance_[14728226207776]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" } ] }, @@ -2976,43 +3348,123 @@ }, { "op": "replace", - "path": "/Instances/Instance_[44675714163097]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[12678007301039]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[12678007301039]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[12678007301039]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[13150453703599]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[13150453703599]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[13150453703599]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44675714163097]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44675714163097]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[44675714163097]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44680009130393]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44680009130393]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[44680009130393]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44954887037337]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44954887037337]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 + }, + { + "op": "replace", + "path": "/Instances/Instance_[44954887037337]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44959182004633]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" + }, + { + "op": "replace", + "path": "/Instances/Instance_[44959182004633]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 }, { "op": "replace", - "path": "/Instances/Instance_[13150453703599]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[44959182004633]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" }, { "op": "replace", - "path": "/Instances/Instance_[44959182004633]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[45496052916633]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" }, { "op": "replace", - "path": "/Instances/Instance_[44954887037337]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[45496052916633]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 }, { "op": "replace", - "path": "/Instances/Instance_[45496052916633]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[45496052916633]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" }, { "op": "replace", - "path": "/Instances/Instance_[45500347883929]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[45500347883929]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/guid", + "value": "{00000000-0000-0000-0000-000000000000}" }, { "op": "replace", - "path": "/Instances/Instance_[44680009130393]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[45500347883929]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetId/subId", + "value": 0 }, { "op": "replace", - "path": "/Instances/Instance_[12678007301039]/Entities/Entity_[1614665180534621]/Components/Component_[14793421525127928751]/Controller/Configuration/materials/0/Value/MaterialAsset/assetHint", - "value": "assets/warehouse/materials/mwarehouseprops.azmaterial" + "path": "/Instances/Instance_[45500347883929]/Entities/Entity_[1614665180534621]/Components/Component_[15264405835034640312]/ColliderConfiguration/MaterialSlots/Slots/0/MaterialAsset/assetHint", + "value": "" } ] }, @@ -3094,4 +3546,4 @@ ] } } -} +} \ No newline at end of file diff --git a/Project/Prefabs/Features/HumanNavigation.prefab b/Project/Prefabs/Features/HumanNavigation.prefab index ae095904..9be737fe 100755 --- a/Project/Prefabs/Features/HumanNavigation.prefab +++ b/Project/Prefabs/Features/HumanNavigation.prefab @@ -1083,11 +1083,36 @@ "Instance_[53741569943658]": { "Source": "HumanWorker/HumanWorkerRobot.prefab", "Patches": [ + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", + "value": "../Entity_[9901338624620]" + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", + "value": -35.32099914550781 + }, + { + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", + "value": 19.8799991607666 + }, { "op": "replace", "path": "/Entities/Entity_[66011467218485]/Name", "value": "HumanWorker1" }, + { + "op": "replace", + "path": "/Entities/Entity_[66011467218485]/Components/EditorShapeColliderComponent/ColliderConfiguration/CollisionGroupId/GroupId", + "value": "{72AB7AF0-F57A-4DA8-8498-3DBCF3789BAE}" + }, + { + "op": "replace", + "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Detour Navigation Entity", + "value": "../Entity_[9879863788140]" + }, { "op": "add", "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Waypoints/0", @@ -1147,12 +1172,12 @@ "op": "add", "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Waypoints/11", "value": "../Entity_[9866978886252]" - }, - { - "op": "replace", - "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Detour Navigation Entity", - "value": "../Entity_[9879863788140]" - }, + } + ] + }, + "Instance_[53758749812842]": { + "Source": "HumanWorker/HumanWorkerRobot.prefab", + "Patches": [ { "op": "replace", "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", @@ -1161,23 +1186,28 @@ { "op": "replace", "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": -35.32099914550781 + "value": -32.257999420166016 }, { "op": "replace", "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 19.8799991607666 - } - ] - }, - "Instance_[53758749812842]": { - "Source": "HumanWorker/HumanWorkerRobot.prefab", - "Patches": [ + "value": 10.576000213623049 + }, { "op": "replace", "path": "/Entities/Entity_[66011467218485]/Name", "value": "HumanWorker2" }, + { + "op": "replace", + "path": "/Entities/Entity_[66011467218485]/Components/EditorShapeColliderComponent/ColliderConfiguration/CollisionGroupId/GroupId", + "value": "{72AB7AF0-F57A-4DA8-8498-3DBCF3789BAE}" + }, + { + "op": "replace", + "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Detour Navigation Entity", + "value": "../Entity_[9879863788140]" + }, { "op": "add", "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Waypoints/0", @@ -1207,26 +1237,6 @@ "op": "add", "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Waypoints/5", "value": "../Entity_[29544175906342]" - }, - { - "op": "replace", - "path": "/Entities/Entity_[66011467218485]/Components/NpcWaypointNavigatorComponent/m_template/Detour Navigation Entity", - "value": "../Entity_[9879863788140]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", - "value": "../Entity_[9901338624620]" - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/0", - "value": -32.257999420166016 - }, - { - "op": "replace", - "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Translate/1", - "value": 10.576000213623049 } ] } diff --git a/Project/Prefabs/Features/Lines.prefab b/Project/Prefabs/Features/Lines.prefab index 18a2b9ca..80d7ed2a 100755 --- a/Project/Prefabs/Features/Lines.prefab +++ b/Project/Prefabs/Features/Lines.prefab @@ -3274,4 +3274,4 @@ ] } } -} +} \ No newline at end of file diff --git a/Project/Prefabs/OTTO600/OTTO600WithPallet.prefab b/Project/Prefabs/OTTO600/OTTO600WithPallet.prefab index 69bc4217..c9c8085b 100755 --- a/Project/Prefabs/OTTO600/OTTO600WithPallet.prefab +++ b/Project/Prefabs/OTTO600/OTTO600WithPallet.prefab @@ -414,7 +414,7 @@ ], "TopicConfiguration": { "Type": "std_msgs::msg::Empty", - "Topic": "/object_detector", + "Topic": "object_detector", "QoS": { "Reliability": 1, "Durability": 2, diff --git a/Project/Prefabs/OTTO600/Otto600StandWithPaths.prefab b/Project/Prefabs/OTTO600/Otto600StandWithPaths.prefab index 01fd6a8e..abb8653e 100644 --- a/Project/Prefabs/OTTO600/Otto600StandWithPaths.prefab +++ b/Project/Prefabs/OTTO600/Otto600StandWithPaths.prefab @@ -346,6 +346,10 @@ "Instance_[52986282575201]": { "Source": "OTTO600/Stand/Otto600Stand.prefab", "Patches": [ + { + "op": "remove", + "path": "/Entities/Entity_[3817754087158]/Components/EditorMeshColliderComponent" + }, { "op": "replace", "path": "/ContainerEntity/Components/TransformComponent/Parent Entity", @@ -367,8 +371,9 @@ "value": 2.384185791015625e-7 }, { - "op": "remove", - "path": "/Entities/Entity_[3817754087158]/Components/EditorMeshColliderComponent" + "op": "replace", + "path": "/ContainerEntity/Components/TransformComponent/Transform Data/Rotate/2", + "value": -90.0 } ] } diff --git a/Project/Prefabs/ur20_cameras.prefab b/Project/Prefabs/ur20_cameras.prefab index a1bf0430..e6af70f9 100755 --- a/Project/Prefabs/ur20_cameras.prefab +++ b/Project/Prefabs/ur20_cameras.prefab @@ -486,7 +486,7 @@ "Translate": [ -1.213998794555664, 4.76837158203125e-7, - 1.3859866857528687 + 1.3859866857528689 ], "Rotate": [ 180.0, diff --git a/Project/project.json b/Project/project.json index 9bfa2b74..fdb4214f 100644 --- a/Project/project.json +++ b/Project/project.json @@ -19,6 +19,10 @@ "external_subdirectories": [ "Gem", "../engine/o3de-extras/Gems/ROS2", + "../engine/o3de-extras/Gems/ROS2Controllers", + "../engine/o3de-extras/Gems/ROS2RobotImporter", + "../engine/o3de-extras/Gems/SimulationInterfaces", + "../engine/o3de-extras/Gems/ROS2Sensors", "../engine/o3de-extras/Gems/LevelGeoreferencing", "../engine/o3de-extras/Gems/WarehouseAssets", "../engine/o3de-extras/Gems/WarehouseAutomation", @@ -37,13 +41,16 @@ "HumanWorker==2.0.0", "OTTORobots==2.0.0", "ROS2>=3.3.0", - "URRobots==2.0.0", - "WarehouseAssets>=2.0.0", - "WarehouseAutomation==2.0.0", - "RobotecWarehouseRacksAssets", + "ROS2Controllers", + "ROS2RobotImporter", + "ROS2ScriptIntegration", + "ROS2Sensors", + "RobotecSmallContainersAssets", "RobotecWarehouseBarriersAndCagesAssets", "RobotecWarehouseForkliftsAssets", - "RobotecSmallContainersAssets", - "ROS2ScriptIntegration" + "RobotecWarehouseRacksAssets", + "URRobots==2.0.0", + "WarehouseAssets>=2.0.0", + "WarehouseAutomation==2.0.0" ] } diff --git a/README.md b/README.md index c7415f0c..634ee132 100644 --- a/README.md +++ b/README.md @@ -131,7 +131,7 @@ The Gems are open to your contributions! ### ROS 2 packages Make sure to install the necessary ROS 2 packages. ```bash -sudo apt install ros-${ROS_DISTRO}-ackermann-msgs ros-${ROS_DISTRO}-control-toolbox ros-${ROS_DISTRO}-nav-msgs ros-${ROS_DISTRO}-gazebo-msgs ros-${ROS_DISTRO}-vision-msgs ros-${ROS_DISTRO}-ur-msgs ros-${ROS_DISTRO}-moveit-servo ros-${ROS_DISTRO}-moveit-visual-tools ros-${ROS_DISTRO}-moveit ros-${ROS_DISTRO}-pilz-industrial-motion-planner ros-${ROS_DISTRO}-controller-manager ros-${ROS_DISTRO}-ur-client-library ros-${ROS_DISTRO}-nav2-common ros-${ROS_DISTRO}-navigation2 libopencv-dev ros-${ROS_DISTRO}-nav2-map-server +sudo apt install ros-${ROS_DISTRO}-ackermann-msgs ros-${ROS_DISTRO}-control-toolbox ros-${ROS_DISTRO}-nav-msgs ros-${ROS_DISTRO}-gazebo-msgs ros-${ROS_DISTRO}-vision-msgs ros-${ROS_DISTRO}-ur-msgs ros-${ROS_DISTRO}-moveit-servo ros-${ROS_DISTRO}-moveit-visual-tools ros-${ROS_DISTRO}-moveit ros-${ROS_DISTRO}-pilz-industrial-motion-planner ros-${ROS_DISTRO}-controller-manager ros-${ROS_DISTRO}-ur-client-library ros-${ROS_DISTRO}-nav2-common ros-${ROS_DISTRO}-navigation2 libopencv-dev ros-${ROS_DISTRO}-nav2-map-server ros-${ROS_DISTRO}-simulation-interfaces ``` ### Project @@ -186,12 +186,11 @@ To build the game launcher and bundle assets: ```bash cd ${RC2023_WORKDIR}/engine/o3de ./scripts/o3de.sh export-project -es ExportScripts/export_source_built_project.py \ - --project-path ${RC2023_WORKDIR}/ROSCon2023Demo/Project \ - --seedlist ${RC2023_WORKDIR}/ROSCon2023Demo/Project/AssetBundling/SeedLists/demo.seed \ + --project-path ${RC2023_WORKDIR}/Project \ + --seedlist ${RC2023_WORKDIR}/Project/AssetBundling/SeedLists/demo.seed \ --fail-on-asset-errors \ -noserver \ -out ${RC2023_WORKDIR}/ROSCon2023Demo/Project/build/release \ - --build-tools \ --no-unified-launcher ``` The build package is available here: @@ -214,8 +213,8 @@ Please consider copying ROS 2 workspace to the release package. The ROS 2 worksp ```bash # Consider copying ROS 2 workspace -mkdir -p ${RC2023_WORKDIR}/ROSCon2023Demo/Project/build/release/ros2_ws/ -cp -r ${RC2023_WORKDIR}/ROSCon2023Demo/ros2_ws/src ${RC2023_WORKDIR}/ROSCon2023Demo/Project/build/release/ros2_ws/ +mkdir -p ${RC2023_WORKDIR}/Project/build/release/ros2_ws/ +cp -r ${RC2023_WORKDIR}/ros2_ws/src ${RC2023_WORKDIR}/ROSCon2023Demo/Project/build/release/ros2_ws/ ``` To start a released the GameLauncher simply: @@ -325,6 +324,15 @@ Please also refer to the common [Troubleshooting Guide](https://docs.o3de.org/do ## Release notes +### ROSCon2023Demo 4.0.0 for O3DE 2604.x and ROS 2 Jazzy +Changes compared to 3.0.0 +- updated demo the newest available version of O3DE and ROS 2 Gem +- Updated code to new API in ROS 2 Gem and other canonical gems in O3DE 2604. +- made flat copy of UR MoveIt2, to avoid issues with building UR ROS2 Driver +- Updated Nav2 configuration for ROS 2 Jazzy + + + ### ROSCon2023Demo 3.0.0 for O3DE 2505.x Changes compared to 2.0.0 - updated demo the newest available version of O3DE and ROS 2 Gem diff --git a/engine/o3de b/engine/o3de index faeeba43..09ac09d2 160000 --- a/engine/o3de +++ b/engine/o3de @@ -1 +1 @@ -Subproject commit faeeba431d2b22facb020608054ddfb4c66b7224 +Subproject commit 09ac09d262e2afe2a58385c08d584145b7148863 diff --git a/engine/o3de-extras b/engine/o3de-extras index 3d6f4e48..e9c2d0aa 160000 --- a/engine/o3de-extras +++ b/engine/o3de-extras @@ -1 +1 @@ -Subproject commit 3d6f4e485eb126a7af64efa4cb541f2911308a0e +Subproject commit e9c2d0aa0cc36ca797b0acdd9e47824d85a64151 diff --git a/gems/o3de-humanworker-gem b/gems/o3de-humanworker-gem index 25abe695..744488e1 160000 --- a/gems/o3de-humanworker-gem +++ b/gems/o3de-humanworker-gem @@ -1 +1 @@ -Subproject commit 25abe695a1cdc0e0bf02507c855d56aedc486187 +Subproject commit 744488e11a5eb87d72f47bf7d0db605074e9a992 diff --git a/gems/o3de-otto-robots-gem b/gems/o3de-otto-robots-gem index 949bfc5d..78881083 160000 --- a/gems/o3de-otto-robots-gem +++ b/gems/o3de-otto-robots-gem @@ -1 +1 @@ -Subproject commit 949bfc5d11fc8797975542b0042cdc00d6dd8de0 +Subproject commit 788810837e803cd2f5a08b28efd1a715972db534 diff --git a/gems/o3de-ur-robots-gem b/gems/o3de-ur-robots-gem index 33fc9f1f..83b70947 160000 --- a/gems/o3de-ur-robots-gem +++ b/gems/o3de-ur-robots-gem @@ -1 +1 @@ -Subproject commit 33fc9f1f0629d7d8dd5378958dae774434c9334d +Subproject commit 83b70947e4fe5febfde3f5c188ce31131a63e548 diff --git a/gems/robotec-o3de-tools b/gems/robotec-o3de-tools index 741cd276..4811db4f 160000 --- a/gems/robotec-o3de-tools +++ b/gems/robotec-o3de-tools @@ -1 +1 @@ -Subproject commit 741cd276177728a78566d327871ab7e342b83567 +Subproject commit 4811db4f29dde08418e9348fd8a2a6d9ee606322 diff --git a/gems/robotec-warehouse-assets b/gems/robotec-warehouse-assets index 42236163..ab6b54a0 160000 --- a/gems/robotec-warehouse-assets +++ b/gems/robotec-warehouse-assets @@ -1 +1 @@ -Subproject commit 4223616381876126cda8a6a8de6b1e9b0f4bdb1b +Subproject commit ab6b54a0f54cbbe6c3fb0d6aa32af18cf071ee40 diff --git a/ros2_ws/setup_submodules.bash b/ros2_ws/setup_submodules.bash deleted file mode 100755 index 147f47cd..00000000 --- a/ros2_ws/setup_submodules.bash +++ /dev/null @@ -1,38 +0,0 @@ -#!/bin/bash - -if [ "$ROS_DISTRO" == "iron" ] -then - git submodule init - git submodule update - - cd ./src/Universal_Robots_ROS2_Driver/ - git checkout 129687763140508b4b5cb497b197ce1057defe88 - if [ $? -ne 0 ] - then - echo "Failed to checkout ROS 2 iron branch." - exit 1 - fi -elif [ "$ROS_DISTRO" == "humble" ] -then - # submodule init and update are sufficient for humble - git submodule init - git submodule update - cd ./src/Universal_Robots_ROS2_Driver/ - git checkout humble - if [ $? -ne 0 ] - then - echo "Failed to checkout ROS 2 humble branch." - exit 1 - fi - -elif [ -z "$ROS_DISTRO" ] -then - echo "ROS 2 distribution not found." - exit 1 -else - echo "ROS 2 $ROS_DISTRO is not supported in this project - switch to humble or iron distribution." - exit 1 -fi - -echo "Submodules updated for ROS2 $ROS_DISTRO." -exit 0 diff --git a/ros2_ws/src/Universal_Robots_ROS2_Driver b/ros2_ws/src/Universal_Robots_ROS2_Driver deleted file mode 160000 index f06092e4..00000000 --- a/ros2_ws/src/Universal_Robots_ROS2_Driver +++ /dev/null @@ -1 +0,0 @@ -Subproject commit f06092e4f32ae1d042459cfaaae96b5c0ea1b21d diff --git a/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_multirobot_params.yaml b/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_multirobot_params.yaml new file mode 100644 index 00000000..3cce78ba --- /dev/null +++ b/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_multirobot_params.yaml @@ -0,0 +1,262 @@ +robot_spawner_client: + ros__parameters: + robot_name: + robot_namespace: + robot_initial_position: [, , ] + +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "/base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + map_topic: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "/likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "/odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + set_initial_pose: True + initial_pose: + x: + y: + z: + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: /base_link + odom_topic: odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + + + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + robot_base_frame: "/base_link" + use_sim_time: True + global_frame: /odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: //scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + robot_base_frame: "/base_link" + global_frame: "map" + use_sim_time: True + robot_radius: 0.22 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: //scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "map_warehouse.yaml" + save_map_timeout: 5.0 + frame_id: "map" + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + robot_base_frame: "/base_link" + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + global_frame: /odom + robot_base_frame: /base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file diff --git a/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_params.yaml b/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_params.yaml new file mode 100644 index 00000000..ab31468c --- /dev/null +++ b/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_params.yaml @@ -0,0 +1,252 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + map_topic: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + set_initial_pose: False + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + + + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + robot_base_frame: "base_link" + use_sim_time: True + global_frame: odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + robot_base_frame: "base_link" + global_frame: "map" + use_sim_time: True + robot_radius: 0.22 + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "map_warehouse.yaml" + save_map_timeout: 5.0 + frame_id: "map" + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + robot_base_frame: "base_link" + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file diff --git a/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_slam.yaml b/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_slam.yaml new file mode 100644 index 00000000..2b8d2171 --- /dev/null +++ b/ros2_ws/src/otto_fleet_nav/params/jazzy/nav2_slam.yaml @@ -0,0 +1,302 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + + + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + width: 50 + height: 50 + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + assisted_teleop: + plugin: "nav2_behaviors::AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 \ No newline at end of file diff --git a/ros2_ws/src/otto_fleet_nav/params/jazzy/otto1500Params.yaml b/ros2_ws/src/otto_fleet_nav/params/jazzy/otto1500Params.yaml new file mode 100644 index 00000000..5e87383d --- /dev/null +++ b/ros2_ws/src/otto_fleet_nav/params/jazzy/otto1500Params.yaml @@ -0,0 +1,292 @@ +robot_spawner_client: + ros__parameters: + robot_name: + robot_namespace: + robot_initial_position: [, , ] + +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "/base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + map_topic: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "/likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "/odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + set_initial_pose: True + initial_pose: + x: + y: + z: + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: /base_link + robot_base_frame_blackboard_id: "/base_link" + odom_topic: odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + default_nav_through_poses_bt_xml: "" + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: -0.7 + min_vel_y: 0.0 + max_vel_x: 0.7 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 1.0 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + robot_base_frame: "/base_link" + use_sim_time: True + global_frame: /odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[ [0.96, 0.61], [-0.96, 0.61],[-0.96, -0.61],[0.96, -0.61]]" + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 20.0 + inflation_radius: 4. + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan_front scan_rear + scan_front: + topic: //scan_front + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + scan_rear: + topic: //scan_rear + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + robot_base_frame: "/base_link" + global_frame: "map" + use_sim_time: True + # robot_radius: 0.22 + footprint: "[ [0.55, 0.35], [-0.55, 0.35],[-0.55, -0.35],[0.55, -0.35]]" + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan_front scan_rear + scan_front: + topic: //scan_front + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + scan_rear: + topic: //scan_rear + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 20.0 + inflation_radius: 5. + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "map_warehouse.yaml" + save_map_timeout: 5.0 + frame_id: "map" + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + + +velocity_smoother: + ros__parameters: + max_velocity: [1.25, 0.0, 1.0] + min_velocity: [-1.25, 0.0, -1.0] + +smoother_server: + ros__parameters: + use_sim_time: True + robot_base_frame: "/base_link" + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + global_frame: /odom + robot_base_frame: /base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file diff --git a/ros2_ws/src/otto_fleet_nav/params/jazzy/otto600Params.yaml b/ros2_ws/src/otto_fleet_nav/params/jazzy/otto600Params.yaml new file mode 100644 index 00000000..5dc607ff --- /dev/null +++ b/ros2_ws/src/otto_fleet_nav/params/jazzy/otto600Params.yaml @@ -0,0 +1,291 @@ +robot_spawner_client: + ros__parameters: + robot_name: + robot_namespace: + robot_initial_position: [, , ] + +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "/base_link" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + map_topic: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "/likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "/odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + set_initial_pose: True + initial_pose: + x: + y: + z: + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: /base_link + robot_base_frame_blackboard_id: "/base_link" + odom_topic: odom + bt_loop_duration: 10 + default_server_timeout: 20 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + default_nav_through_poses_bt_xml: "" + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator::NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["goal_checker"] + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: -0.7 + min_vel_y: 0.0 + max_vel_x: 0.7 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 1.0 + min_speed_theta: 0.0 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 0.0 + GoalAlign.scale: 0.0 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + robot_base_frame: "/base_link" + use_sim_time: True + global_frame: /odom + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + footprint: "[ [0.55, 0.35], [-0.55, 0.35],[-0.55, -0.35],[0.55, -0.35]]" + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 20.0 + inflation_radius: 4. + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: False + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan_front scan_rear + scan_front: + topic: //scan_front + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + scan_rear: + topic: //scan_rear + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + robot_base_frame: "/base_link" + global_frame: "map" + use_sim_time: True + # robot_radius: 0.22 + footprint: "[ [0.55, 0.35], [-0.55, 0.35],[-0.55, -0.35],[0.55, -0.35]]" + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan_front scan_rear + scan_front: + topic: //scan_front + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + scan_rear: + topic: //scan_rear + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 20.0 + inflation_radius: 5. + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + yaml_filename: "map_warehouse.yaml" + save_map_timeout: 5.0 + frame_id: "map" + +planner_server: + ros__parameters: + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner::NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + robot_base_frame: "/base_link" + +velocity_smoother: + ros__parameters: + max_velocity: [1.25, 0.0, 1.0] + min_velocity: [-1.25, 0.0, -1.0] + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "wait"] + spin: + plugin: "nav2_behaviors::Spin" + backup: + plugin: "nav2_behaviors::BackUp" + drive_on_heading: + plugin: "nav2_behaviors::DriveOnHeading" + wait: + plugin: "nav2_behaviors::Wait" + global_frame: /odom + robot_base_frame: /base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 \ No newline at end of file diff --git a/ros2_ws/src/otto_fleet_nav/setup.py b/ros2_ws/src/otto_fleet_nav/setup.py index 63e0ca67..baa447a2 100644 --- a/ros2_ws/src/otto_fleet_nav/setup.py +++ b/ros2_ws/src/otto_fleet_nav/setup.py @@ -17,6 +17,7 @@ (os.path.join('share', package_name, 'maps'), glob(os.path.join('maps', '*.yaml'))), (os.path.join('share', package_name, 'params', 'iron'), glob(os.path.join('params', 'iron', '*.yaml'))), (os.path.join('share', package_name, 'params', 'humble'), glob(os.path.join('params', 'humble', '*.yaml'))), + (os.path.join('share', package_name, 'params', 'jazzy'), glob(os.path.join('params', 'jazzy', '*.yaml'))), (os.path.join('share', package_name, 'rviz'), glob(os.path.join('rviz', '*.rviz'))), (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.yaml'))), (os.path.join('share', package_name, 'behaviour_trees'), glob(os.path.join('behaviour_trees', '*.xml'))) diff --git a/ros2_ws/src/roscon2023_demo/config/ROSCon2023Config_4.yaml b/ros2_ws/src/roscon2023_demo/config/ROSCon2023Config_4.yaml index c30edfc1..952af90a 100644 --- a/ros2_ws/src/roscon2023_demo/config/ROSCon2023Config_4.yaml +++ b/ros2_ws/src/roscon2023_demo/config/ROSCon2023Config_4.yaml @@ -42,17 +42,17 @@ fleet: arms: - namespace: ur1 - num_of_boxes: 18 + num_of_boxes: 12 launch_rviz: false - namespace: ur2 - num_of_boxes: 18 + num_of_boxes: 12 launch_rviz: false - namespace: ur3 - num_of_boxes: 18 + num_of_boxes: 12 launch_rviz: false - namespace: ur4 - num_of_boxes: 18 + num_of_boxes: 12 launch_rviz: false \ No newline at end of file diff --git a/ros2_ws/src/ur_moveit_config/CHANGELOG.rst b/ros2_ws/src/ur_moveit_config/CHANGELOG.rst new file mode 100644 index 00000000..12234f56 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/CHANGELOG.rst @@ -0,0 +1,108 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package ur_moveit_config +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.2 (2025-01-21) +------------------ +* ur_moveit_config: Do not change default controller when using fake hardware (`#1237 `_) +* Contributors: Felix Exner + +2.5.1 (2024-12-21) +------------------ + +2.5.0 (2024-12-18) +------------------ +* Update package maintainers (backport of `#1203 `_) +* Contributors: mergify[bot] + +2.2.16 (2024-10-28) +------------------- +* Properly handle use_sim_time (`#1146 `_) (`#1159 `_) +* Disable execution_duration_monitoring by default (`#1133 `_) +* Added option to publish SRDF file. +* Contributors: Felix Exner, mergify[bot], v-marsh + +2.2.15 (2024-07-26) +------------------- + +2.2.14 (2024-07-01) +------------------- + +2.2.13 (2024-06-17) +------------------- +* Add servo node config to disable advertising /get_planning_scene (backport of `#990 `_) +* Contributors: Ruddick Lawrence + +2.2.12 (2024-05-16) +------------------- +* Define default maximum accelerations for MoveIt (backport of `#645 `_) +* Fix multi-line strings in DeclareLaunchArgument (`#948 `_) +* Contributors: Robert Wilbrandt, Matthijs van der Burgh + +2.2.11 (2024-04-08) +------------------- +* Add UR30 support (`#930 `_) +* Contributors: Felix Exner, Vincenzo Di Pentima + +2.2.10 (2024-01-03) +------------------- + +2.2.9 (2023-09-22) +------------------ +* Added support for UR20 (`#805 `_) +* Contributors: mergify[bot], Felix Exner + +2.2.8 (2023-06-26) +------------------ + +2.2.7 (2023-06-02) +------------------ +* Update linters & checkers (backport `#426 `_) (`#556 `_) +* Contributors: mergify[bot], Felix Exner + +2.2.6 (2022-11-28) +------------------ + +2.2.5 (2022-11-19) +------------------ + +2.2.4 (2022-10-07) +------------------ +* Fix selecting the right controller given fake_hw + This was falsely introduced earlier. This is a working version. +* add ur_moveit.launch.py parameter to use working controller when using fake hardware (`#464 `_) + add script parameter to use correct controller when using fake hardware +* Contributors: Felix Exner, adverley + +2.2.3 (2022-07-27) +------------------ + +2.2.2 (2022-07-19) +------------------ +* Made sure all past maintainers are listed as authors (`#429 `_) +* Contributors: Felix Exner + +2.2.1 (2022-06-27) +------------------ +* Remove non-required dependency from CMakeLists (`#414 `_) +* Contributors: Felix Exner + +2.2.0 (2022-06-20) +------------------ +* Updated package maintainers +* Prepare for humble (`#394 `_) +* Update dependencies on all packages (`#391 `_) +* Replace warehouse_ros_mongo with warehouse_ros_sqlite (`#362 `_) +* Add missing dep to warehouse_ros_mongo (`#352 `_) +* Update license to BSD-3-Clause (`#277 `_) +* Correct loading kinematics parameters from yaml (`#308 `_) +* Update MoveIt file for working with simulation. (`#278 `_) +* Changing default controller in MoveIt config. (`#288 `_) +* Move Servo launching into the main MoveIt launch file. Make it optional. (`#239 `_) +* Joint limits parameters for Moveit planning (`#187 `_) +* Update Servo parameters, for smooth motion (`#188 `_) +* Enabling velocity mode (`#146 `_) +* Remove obsolete and unused files and packages. (`#80 `_) +* Review CI by correcting the configurations (`#71 `_) +* Add support for gpios, update MoveIt and ros2_control launching (`#66 `_) +* Contributors: AndyZe, Denis Štogl, Felix Exner, livanov93, Robert Wilbrandt diff --git a/ros2_ws/src/ur_moveit_config/CMakeLists.txt b/ros2_ws/src/ur_moveit_config/CMakeLists.txt new file mode 100644 index 00000000..e47ea764 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.5) +project(ur_moveit_config) + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +install(DIRECTORY config launch rviz srdf + DESTINATION share/${PROJECT_NAME} +) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) +ament_python_install_module(${PROJECT_NAME}/launch_common.py) + +ament_package() diff --git a/ros2_ws/src/ur_moveit_config/config/controllers.yaml b/ros2_ws/src/ur_moveit_config/config/controllers.yaml new file mode 100644 index 00000000..784ebd31 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/config/controllers.yaml @@ -0,0 +1,29 @@ +controller_names: + - scaled_joint_trajectory_controller + - joint_trajectory_controller + + +scaled_joint_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: true + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + + +joint_trajectory_controller: + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: false + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ros2_ws/src/ur_moveit_config/config/joint_limits.yaml b/ros2_ws/src/ur_moveit_config/config/joint_limits.yaml new file mode 100644 index 00000000..dc216949 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/config/joint_limits.yaml @@ -0,0 +1,25 @@ +# These limits are used by MoveIt and augment/override the definitions in ur_description. +# +# While the robot does not inherently have any limits on joint accelerations (only on torques), +# MoveIt needs them for time parametrization. They were chosen conservatively to work in most use +# cases. For specific applications, higher values might lead to better execution performance. + +joint_limits: + shoulder_pan_joint: + has_acceleration_limits: true + max_acceleration: 5.0 + shoulder_lift_joint: + has_acceleration_limits: true + max_acceleration: 5.0 + elbow_joint: + has_acceleration_limits: true + max_acceleration: 5.0 + wrist_1_joint: + has_acceleration_limits: true + max_acceleration: 5.0 + wrist_2_joint: + has_acceleration_limits: true + max_acceleration: 5.0 + wrist_3_joint: + has_acceleration_limits: true + max_acceleration: 5.0 diff --git a/ros2_ws/src/ur_moveit_config/config/kinematics.yaml b/ros2_ws/src/ur_moveit_config/config/kinematics.yaml new file mode 100644 index 00000000..af3d30e8 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/config/kinematics.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + robot_description_kinematics: + ur_manipulator: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 diff --git a/ros2_ws/src/ur_moveit_config/config/ompl_planning.yaml b/ros2_ws/src/ur_moveit_config/config/ompl_planning.yaml new file mode 100644 index 00000000..4dcc7a77 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,70 @@ +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expression. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar +ur_manipulator: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + ##Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE + #projection_evaluator: joints(shoulder_pan_joint,shoulder_lift_joint) + longest_valid_segment_fraction: 0.01 diff --git a/ros2_ws/src/ur_moveit_config/config/ur_servo.yaml b/ros2_ws/src/ur_moveit_config/config/ur_servo.yaml new file mode 100644 index 00000000..3b1b0ba1 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/config/ur_servo.yaml @@ -0,0 +1,82 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.01 +# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level +# controller performance. It essentially increases the timestep when calculating the target pose, to move the target +# pose farther away. [seconds] +system_latency_compensation: 0.04 + +## Properties of outgoing commands +publish_period: 0.004 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: std_msgs/Float64MultiArray + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: false + +## Incoming Joint State properties +low_pass_filter_coeff: 10.0 # Larger --> trust the filtered data more, trust the measurements less. + +## MoveIt properties +move_group_name: ur_manipulator # Often 'manipulator' or 'arm' +planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: tool0 # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 100.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /forward_position_controller/commands # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: threshold_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/ros2_ws/src/ur_moveit_config/launch/ur_moveit.launch.py b/ros2_ws/src/ur_moveit_config/launch/ur_moveit.launch.py new file mode 100644 index 00000000..3ceb5558 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/launch/ur_moveit.launch.py @@ -0,0 +1,382 @@ +# Copyright (c) 2021 PickNik, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Denis Stogl + +import os + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ur_moveit_config.launch_common import load_yaml + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.conditions import IfCondition +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) + + +def launch_setup(context, *args, **kwargs): + + # Initialize Arguments + ur_type = LaunchConfiguration("ur_type") + safety_limits = LaunchConfiguration("safety_limits") + safety_pos_margin = LaunchConfiguration("safety_pos_margin") + safety_k_position = LaunchConfiguration("safety_k_position") + # General arguments + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + _publish_robot_description_semantic = LaunchConfiguration("publish_robot_description_semantic") + moveit_config_package = LaunchConfiguration("moveit_config_package") + moveit_joint_limits_file = LaunchConfiguration("moveit_joint_limits_file") + moveit_config_file = LaunchConfiguration("moveit_config_file") + warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path") + prefix = LaunchConfiguration("prefix") + use_sim_time = LaunchConfiguration("use_sim_time") + launch_rviz = LaunchConfiguration("launch_rviz") + launch_servo = LaunchConfiguration("launch_servo") + + joint_limit_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"] + ) + kinematics_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"] + ) + physical_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"] + ) + visual_params = PathJoinSubstitution( + [FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]), + " ", + "robot_ip:=xxx.yyy.zzz.www", + " ", + "joint_limit_params:=", + joint_limit_params, + " ", + "kinematics_params:=", + kinematics_params, + " ", + "physical_params:=", + physical_params, + " ", + "visual_params:=", + visual_params, + " ", + "safety_limits:=", + safety_limits, + " ", + "safety_pos_margin:=", + safety_pos_margin, + " ", + "safety_k_position:=", + safety_k_position, + " ", + "name:=", + "ur", + " ", + "ur_type:=", + ur_type, + " ", + "script_filename:=ros_control.urscript", + " ", + "input_recipe_filename:=rtde_input_recipe.txt", + " ", + "output_recipe_filename:=rtde_output_recipe.txt", + " ", + "prefix:=", + prefix, + " ", + ] + ) + robot_description = {"robot_description": robot_description_content} + + # MoveIt Configuration + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "srdf", moveit_config_file] + ), + " ", + "name:=", + # Also ur_type parameter could be used but then the planning group names in yaml + # configs has to be updated! + "ur", + " ", + "prefix:=", + prefix, + " ", + ] + ) + robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content} + + publish_robot_description_semantic = { + "publish_robot_description_semantic": _publish_robot_description_semantic + } + + robot_description_kinematics = PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "config", "kinematics.yaml"] + ) + + robot_description_planning = { + "robot_description_planning": load_yaml( + str(moveit_config_package.perform(context)), + os.path.join("config", str(moveit_joint_limits_file.perform(context))), + ) + } + + # Planning Configuration + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml") + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Trajectory Execution Configuration + controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml") + # the scaled_joint_trajectory_controller does not work on fake hardware + change_controllers = context.perform_substitution(use_sim_time) + if change_controllers == "true": + controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False + controllers_yaml["joint_trajectory_controller"]["default"] = True + + moveit_controllers = { + "moveit_simple_controller_manager": controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + trajectory_execution = { + "moveit_manage_controllers": False, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + # Execution time monitoring can be incompatible with the scaled JTC + "trajectory_execution.execution_duration_monitoring": False, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + warehouse_ros_config = { + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "warehouse_host": warehouse_sqlite_path, + } + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + publish_robot_description_semantic, + robot_description_kinematics, + robot_description_planning, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {"use_sim_time": use_sim_time}, + warehouse_ros_config, + ], + ) + + # rviz with moveit configuration + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"] + ) + rviz_node = Node( + package="rviz2", + condition=IfCondition(launch_rviz), + executable="rviz2", + name="rviz2_moveit", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + robot_description_kinematics, + robot_description_planning, + warehouse_ros_config, + { + "use_sim_time": use_sim_time, + }, + ], + ) + + # Servo node for realtime control + servo_yaml = load_yaml("ur_moveit_config", "config/ur_servo.yaml") + servo_params = {"moveit_servo": servo_yaml} + servo_node = Node( + package="moveit_servo", + condition=IfCondition(launch_servo), + executable="servo_node_main", + parameters=[ + servo_params, + robot_description, + robot_description_semantic, + ], + output="screen", + ) + + nodes_to_start = [move_group_node, rviz_node, servo_node] + + return nodes_to_start + + +def generate_launch_description(): + + declared_arguments = [] + # UR specific arguments + declared_arguments.append( + DeclareLaunchArgument( + "ur_type", + description="Type/series of used UR robot.", + choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"], + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_limits", + default_value="true", + description="Enables the safety limits controller if true.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_pos_margin", + default_value="0.15", + description="The margin to lower and upper limits in the safety controller.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "safety_k_position", + default_value="20", + description="k-position factor in the safety controller.", + ) + ) + # General arguments + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="ur_description", + description="Description package with robot URDF/XACRO files. Usually the argument " + "is not set, it enables use of a custom description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="ur.urdf.xacro", + description="URDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "publish_robot_description_semantic", + default_value="True", + description="Whether to publish the SRDF description on topic /robot_description_semantic.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_package", + default_value="ur_moveit_config", + description="MoveIt config package with robot SRDF/XACRO files. Usually the argument " + "is not set, it enables use of a custom moveit config.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_config_file", + default_value="ur.srdf.xacro", + description="MoveIt SRDF/XACRO description file with the robot.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "moveit_joint_limits_file", + default_value="joint_limits.yaml", + description="MoveIt joint limits that augment or override the values from the URDF robot_description.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "warehouse_sqlite_path", + default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"), + description="Path where the warehouse database should be stored", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "use_sim_time", + default_value="false", + description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument( + "prefix", + default_value='""', + description="Prefix of the joint names, useful for " + "multi-robot setup. If changed than also joint names in the controllers' configuration " + "have to be updated.", + ) + ) + declared_arguments.append( + DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?") + ) + declared_arguments.append( + DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?") + ) + + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) diff --git a/ros2_ws/src/ur_moveit_config/package.xml b/ros2_ws/src/ur_moveit_config/package.xml new file mode 100644 index 00000000..fa9e18e9 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/package.xml @@ -0,0 +1,41 @@ + + + + ur_moveit_config + 2.5.2 + + An example package with MoveIt2 configurations for UR robots. + + + Felix Exner + Rune Søe-Knudsen + Universal Robots A/S + + Apache2.0 + + Lovro Ivanov + Andy Zelenak + Vincenzo Di Pentima + Denis Stogl + + ament_cmake + ament_cmake_python + + launch + launch_ros + moveit_kinematics + moveit_planners_ompl + moveit_ros_move_group + moveit_ros_visualization + moveit_servo + moveit_simple_controller_manager + rviz2 + ur_description + urdf + warehouse_ros_sqlite + xacro + + + ament_cmake + + diff --git a/ros2_ws/src/ur_moveit_config/rviz/view_robot.rviz b/ros2_ws/src/ur_moveit_config/rviz/view_robot.rviz new file mode 100644 index 00000000..47e8f87d --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/rviz/view_robot.rviz @@ -0,0 +1,311 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 87 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.4957627058029175 + Tree Height: 573 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: true + Interrupt Display: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: ur_manipulator + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flange: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 6.619869709014893 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.2489434778690338 + Y: -0.013962505385279655 + Z: 0.13800443708896637 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.4103981554508209 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.210397720336914 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1381 + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001e0000004f6fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000002e3000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032a00000212000001b9010000030000000100000110000004f6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000046000004f6000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000070e000004f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 30 diff --git a/ros2_ws/src/ur_moveit_config/srdf/ur.srdf.xacro b/ros2_ws/src/ur_moveit_config/srdf/ur.srdf.xacro new file mode 100644 index 00000000..0ae2cca2 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/srdf/ur.srdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/ros2_ws/src/ur_moveit_config/srdf/ur_macro.srdf.xacro b/ros2_ws/src/ur_moveit_config/srdf/ur_macro.srdf.xacro new file mode 100644 index 00000000..04554f33 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/srdf/ur_macro.srdf.xacro @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros2_ws/src/ur_moveit_config/ur_moveit_config/__init__.py b/ros2_ws/src/ur_moveit_config/ur_moveit_config/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2_ws/src/ur_moveit_config/ur_moveit_config/launch_common.py b/ros2_ws/src/ur_moveit_config/ur_moveit_config/launch_common.py new file mode 100644 index 00000000..c680e878 --- /dev/null +++ b/ros2_ws/src/ur_moveit_config/ur_moveit_config/launch_common.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2021 PickNik, Inc. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +# +# Author: Lovro Ivanov + +import math +import os +import yaml + +from ament_index_python.packages import get_package_share_directory + + +def construct_angle_radians(loader, node): + """Utility function to construct radian values from yaml.""" + value = loader.construct_scalar(node) + try: + return float(value) + except SyntaxError: + raise Exception("invalid expression: %s" % value) + + +def construct_angle_degrees(loader, node): + """Utility function for converting degrees into radians from yaml.""" + return math.radians(construct_angle_radians(loader, node)) + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + yaml.SafeLoader.add_constructor("!radians", construct_angle_radians) + yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees) + except Exception: + raise Exception("yaml support not available; install python-yaml") + + try: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml_abs(absolute_file_path): + + try: + yaml.SafeLoader.add_constructor("!radians", construct_angle_radians) + yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees) + except Exception: + raise Exception("yaml support not available; install python-yaml") + + try: + with open(absolute_file_path) as file: + return yaml.safe_load(file) + except OSError: # parent of IOError, OSError *and* WindowsError where available + return None diff --git a/ros2_ws/src/ur_palletization/CMakeLists.txt b/ros2_ws/src/ur_palletization/CMakeLists.txt index f45e8c51..d807d57c 100644 --- a/ros2_ws/src/ur_palletization/CMakeLists.txt +++ b/ros2_ws/src/ur_palletization/CMakeLists.txt @@ -78,6 +78,7 @@ install(TARGETS palletizationNode install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/ros2_ws/src/ur_palletization/config/sensors_3d.yaml b/ros2_ws/src/ur_palletization/config/sensors_3d.yaml new file mode 100644 index 00000000..2c6318b6 --- /dev/null +++ b/ros2_ws/src/ur_palletization/config/sensors_3d.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + sensors: + - "noop" + noop: + sensor_plugin: "~" diff --git a/ros2_ws/src/ur_palletization/launch/ur_palletization.launch.py b/ros2_ws/src/ur_palletization/launch/ur_palletization.launch.py index d53827ac..4f0d78a0 100644 --- a/ros2_ws/src/ur_palletization/launch/ur_palletization.launch.py +++ b/ros2_ws/src/ur_palletization/launch/ur_palletization.launch.py @@ -44,6 +44,7 @@ from launch.conditions import IfCondition from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile from launch_ros.substitutions import FindPackageShare from moveit_configs_utils import MoveItConfigsBuilder from ur_moveit_config.launch_common import load_yaml @@ -205,6 +206,12 @@ def launch_setup(context, *args, **kwargs): "publish_transforms_updates": True, } + # Disable octomap (no 3D sensors in simulation) + sensor_manager_parameters = ParameterFile( + PathJoinSubstitution([FindPackageShare("ur_palletization"), "config", "sensors_3d.yaml"]), + allow_substs=False, + ) + warehouse_ros_config = { "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", "warehouse_host": warehouse_sqlite_path, @@ -219,7 +226,7 @@ def launch_setup(context, *args, **kwargs): "pilz_industrial_motion_planner": { "default_planner_config": "PTP", - "planning_plugin": "pilz_industrial_motion_planner/CommandPlanner" + "planning_plugins": ["pilz_industrial_motion_planner/CommandPlanner"] }, "robot_description_planning":{ "cartesian_limits":{ @@ -272,6 +279,7 @@ def launch_setup(context, *args, **kwargs): trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, + sensor_manager_parameters, {"use_sim_time": use_sim_time}, warehouse_ros_config, pilz_config @@ -293,6 +301,7 @@ def launch_setup(context, *args, **kwargs): trajectory_execution, moveit_controllers, planning_scene_monitor_parameters, + sensor_manager_parameters, {"use_sim_time": use_sim_time}, warehouse_ros_config, {'publish_robot_description': True}, diff --git a/ros2_ws/src/ur_palletization/src/taskConstructor.cpp b/ros2_ws/src/ur_palletization/src/taskConstructor.cpp index 95dc9e9d..403bf3b7 100644 --- a/ros2_ws/src/ur_palletization/src/taskConstructor.cpp +++ b/ros2_ws/src/ur_palletization/src/taskConstructor.cpp @@ -57,7 +57,7 @@ namespace Palletization if ((errorCode = m_moveGroupInterface->plan(plan)) != moveit::core::MoveItErrorCode::SUCCESS || (errorCode = m_moveGroupInterface->execute(plan)) != moveit::core::MoveItErrorCode::SUCCESS) { - RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to execute move plan for pose " << poseName << " (error code: " << moveit::core::error_code_to_string(errorCode) << ")"); + RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to execute move plan for pose " << poseName << " (error code: " << moveit::core::errorCodeToString(errorCode) << ")"); return false; } return true; @@ -79,7 +79,7 @@ namespace Palletization if ((errorCode = m_moveGroupInterface->plan(plan)) != moveit::core::MoveItErrorCode::SUCCESS || (errorCode = m_moveGroupInterface->execute(plan)) != moveit::core::MoveItErrorCode::SUCCESS) { - RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to execute move plan (error code: " << moveit::core::error_code_to_string(errorCode) << ")"); + RCLCPP_ERROR_STREAM(m_node->get_logger(), "Failed to execute move plan (error code: " << moveit::core::errorCodeToString(errorCode) << ")"); return false; } return true;