From 6a9b6d698a38cca092901257ca4343d8b2a40176 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 18 Oct 2021 13:23:26 -0700 Subject: [PATCH] Add follow tests using Ignition Gazebo's test fixture and run on GitHub actions (#40) --- .github/workflows/build-and-test.sh | 54 +++++ .github/workflows/ci.yml | 34 +++ .gitignore | 1 + .travis.yml | 27 --- .travis/build | 44 ---- README.md | 15 +- dolly_ignition/CMakeLists.txt | 21 +- .../models/dolly_ignition/model.sdf | 2 +- dolly_tests/CHANGELOG.rst | 4 + dolly_tests/CMakeLists.txt | 61 +++++ .../launch/follow_ignition_TEST.launch.py | 92 ++++++++ dolly_tests/package.xml | 33 +++ dolly_tests/test/constants.hh.in | 20 ++ dolly_tests/test/follow_ignition_TEST.cpp | 223 ++++++++++++++++++ dolly_tests/worlds/empty.sdf | 44 ++++ 15 files changed, 593 insertions(+), 82 deletions(-) create mode 100755 .github/workflows/build-and-test.sh create mode 100644 .github/workflows/ci.yml create mode 100644 .gitignore delete mode 100644 .travis.yml delete mode 100755 .travis/build create mode 100644 dolly_tests/CHANGELOG.rst create mode 100644 dolly_tests/CMakeLists.txt create mode 100644 dolly_tests/launch/follow_ignition_TEST.launch.py create mode 100644 dolly_tests/package.xml create mode 100644 dolly_tests/test/constants.hh.in create mode 100644 dolly_tests/test/follow_ignition_TEST.cpp create mode 100644 dolly_tests/worlds/empty.sdf diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh new file mode 100755 index 0000000..b7eabe7 --- /dev/null +++ b/.github/workflows/build-and-test.sh @@ -0,0 +1,54 @@ +#!/bin/bash +set -ev +set -x + +# Configuration. +export COLCON_WS=~/ws +export COLCON_WS_SRC=${COLCON_WS}/src +export DEBIAN_FRONTEND=noninteractive +export ROS_PYTHON_VERSION=3 + +apt update -qq +apt install -qq -y lsb-release wget curl build-essential + +# Fortres isn't on packages.ros.org yet, so we get it from packages.osrfoundation.org +# Once it's on packages.ros.org, it can be installed with rosdep below +if [ "$IGNITION_VERSION" == "fortress" ]; then + echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list + wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - + + IGN_DEPS="ignition-fortress" +fi + +# Tools and dependencies +echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list +curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - +apt-get update -qq +apt-get install -y $IGN_DEPS \ + python3-colcon-common-extensions \ + python3-rosdep \ + xvfb + +rosdep init +rosdep update + +# Create workspace and copy Dolly code there +mkdir -p $COLCON_WS_SRC +cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC + +# Install ROS dependencies +rosdep install --from-paths $COLCON_WS_SRC -i -y -r --rosdistro $ROS_DISTRO + +# Rendering setup +Xvfb :1 -ac -noreset -core -screen 0 1280x1024x24 & +export DISPLAY=:1.0 +export MESA_GL_VERSION_OVERRIDE=3.3 + +# Build +source /opt/ros/$ROS_DISTRO/setup.bash +cd $COLCON_WS +colcon build --event-handlers console_direct+ + +# Test +colcon test --event-handlers console_direct+ --packages-select-regex dolly +colcon test-result diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..250c2fe --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,34 @@ +name: Build and test + +on: [push, pull_request] + +jobs: + build_and_test: + name: Build and test + runs-on: ubuntu-latest + strategy: + matrix: + include: + - docker-image: "ubuntu:20.04" + ignition-version: "fortress" + ros-distro: "rolling" + container: + image: ${{ matrix.docker-image }} + steps: + - name: Checkout dolly + uses: actions/checkout@v2 + with: + path: main + # Compiling ros_ign from source because Rolling isn't using Fortress yet + - name: Checkout ros_ign + uses: actions/checkout@v2 + with: + repository: ignitionrobotics/ros_ign + ref: ros2 + path: ros_ign + - name: Build and test + run: main/.github/workflows/build-and-test.sh + env: + DOCKER_IMAGE: ${{ matrix.docker-image }} + IGNITION_VERSION: ${{ matrix.ignition-version }} + ROS_DISTRO: ${{ matrix.ros-distro }} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c18dd8d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 36f2f62..0000000 --- a/.travis.yml +++ /dev/null @@ -1,27 +0,0 @@ -sudo: required - -env: - matrix: - - DOCKER_IMAGE=ubuntu:20.04 - -services: - - docker - -language: c++ - -compiler: - - gcc - -before_install: - - echo $DOCKER_IMAGE - - docker pull $DOCKER_IMAGE - - docker run -d -v $(pwd):/code $DOCKER_IMAGE /bin/bash -c 'while true; do sleep 1; done' - - docker exec $(docker ps -aq) /bin/bash -c 'id' - -before_script: - - docker exec $(docker ps -aq) /bin/bash -c 'apt-get -qq update' - - docker exec $(docker ps -aq) /bin/bash -c 'apt-get install -y --force-yes build-essential' - -script: - - docker exec $(docker ps -aq) /bin/bash -c 'cd /code && ls -a && ./.travis/build' - diff --git a/.travis/build b/.travis/build deleted file mode 100755 index 2f5b746..0000000 --- a/.travis/build +++ /dev/null @@ -1,44 +0,0 @@ -#!/bin/bash -set -ev - -# Configuration -export COLCON_WS=~/ws -export COLCON_WS_SRC=${COLCON_WS}/src -export DEBIAN_FRONTEND=noninteractive - -# Install system dependencies -apt-get update -qq -apt-get install -qq -y \ - curl \ - git \ - gnupg2 \ - lsb-release -curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - - -echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list - -apt-get update -qq -apt-get install -qq -y \ - python3-colcon-common-extensions \ - python3-rosdep - -# Get ros_ign (TODO: remove once released upstream) -mkdir -p $COLCON_WS_SRC -ln -s /code $COLCON_WS_SRC -cd $COLCON_WS_SRC -git clone https://github.com/ignitionrobotics/ros_ign -b ros2 - -# Install ROS, Gazebo and Ignition -cd $COLCON_WS -rosdep init -rosdep update -rosdep install --from-paths src -i -y --rosdistro foxy - -# Build -source /opt/ros/foxy/setup.bash -cd $COLCON_WS -colcon build - -# Tests -colcon test --packages-select-regex dolly -colcon test-result diff --git a/README.md b/README.md index a9a1305..ccc936a 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -[![Build Status](https://travis-ci.org/chapulina/dolly.svg?branch=master)](https://travis-ci.org/chapulina/dolly) +[![Build and test](https://github.com/chapulina/dolly/actions/workflows/ci.yml/badge.svg)](https://github.com/chapulina/dolly/actions/workflows/ci.yml) # Dolly the robot @@ -21,7 +21,7 @@ Branch | ROS | Gazebo-classic | Ignition | OS [dashing](https://github.com/chapulina/dolly/tree/dashing) | Dashing | Gazebo 9 | :x: | Ubuntu Bionic, macOS Sierra [eloquent](https://github.com/chapulina/dolly/tree/eloquent) | Eloquent | Gazebo 9, Gazebo 11 | Citadel | Ubuntu Bionic [foxy](https://github.com/chapulina/dolly/tree/foxy) | Foxy | Gazebo 11 | Citadel | Ubuntu Focal -[galactic](https://github.com/chapulina/dolly/tree/galactic) | Galactic, Rolling | Gazebo 11 | Edifice | Ubuntu Focal +[galactic](https://github.com/chapulina/dolly/tree/galactic) | Galactic, Rolling | Gazebo 11 | Edifice, Fortress | Ubuntu Focal ## Packages @@ -31,6 +31,7 @@ This repository contains the following packages: * `dolly_follow`: Provides node with follow logic. * `dolly_gazebo`: Robot model, simulation world and launch scripts for Gazebo-classic. * `dolly_ignition`: Robot model, simulation world and launch scripts for Ignition. +* `dolly_tests`: Simulation-based automated tests ## Install @@ -128,11 +129,15 @@ should be enabled. * ⌨️ [Source code](https://github.com/chapulina/simslides/tree/QConSF_Nov2018) * InfoQ * 📰 [Open Source Robotics: Getting Started with Gazebo and ROS 2](https://www.infoq.com/articles/ros-2-gazebo-tutorial/) -* ROS Developers Live Class - * 🎥 [#70 How to Control a Robot with ROS2 (Dashing)](https://www.youtube.com/watch?v=qB4SaP3TZog) - * 🎥 [#71 How to visualize sensor data in ROS2](https://www.youtube.com/watch?v=s3fBGSpmER0) +* The Construct's ROS tutorials + * 🎥 [ROS Developers LIVE Class #70: How to Control a Robot with ROS2 (Dashing)](https://www.youtube.com/watch?v=qB4SaP3TZog) + * 🎥 [ROS Developers Live Class n.71: How to visualize sensor data in ROS2](https://www.youtube.com/watch?v=s3fBGSpmER0) + * 🎥 [Exploring ROS2 with wheeled robot - #1 - Launch ROS2 Simulation](https://www.youtube.com/watch?v=T4iRJqESQAk) + * 🎥 [Exploring ROS2 with wheeled robot - #2 - How to subscribe to ROS2 laser scan topic](https://www.youtube.com/watch?v=2-qO79V_Cik) + * 🎥 [Exploring ROS2 using wheeled Robot - #3 - Moving the Robot](https://www.youtube.com/watch?v=SinvFQ9Vobg) * ROSConJP 2019 * 🎥 [これからのGazebo: ROSのシミュレーションの次世代](https://vimeo.com/370247782) * ⌨️ [Source code](https://github.com/chapulina/rosconjp_2019) * ROS Developers Day 2020 + * 🎥 [Hands-on with Ignition and ROS2 | ROSDevDay2020 Trailer #1](https://www.youtube.com/watch?v=VO0ZUrr7ib8) * 🎥 [Hands-on with Ignition and ROS2](https://youtu.be/nLp4uzN5NMs?t=622) diff --git a/dolly_ignition/CMakeLists.txt b/dolly_ignition/CMakeLists.txt index 6339c06..9ce820d 100644 --- a/dolly_ignition/CMakeLists.txt +++ b/dolly_ignition/CMakeLists.txt @@ -2,11 +2,22 @@ cmake_minimum_required(VERSION 3.5) project(dolly_ignition) -# Skip if Ignition not present -find_package(ignition-gazebo5 QUIET) -if(NOT ignition-gazebo5_FOUND) - message(WARNING "Ignition Gazebo 5 not found, proceeding without that simulator.") - return() +# Fortress +if("$ENV{IGNITION_VERSION}" STREQUAL "fortress") + find_package(ignition-gazebo6 REQUIRED) + + message(STATUS "Using Ignition Fortress") +# Default to Edifice +else() + find_package(ignition-gazebo5 QUIET) + + if(NOT ignition-gazebo5_FOUND) + # Skip if Ignition not present + message(WARNING "Ignition Gazebo 5 or 6 not found, proceeding without that simulator.") + return() + else() + message(STATUS "Using Ignition Edifice") + endif() endif() find_package(ament_cmake REQUIRED) diff --git a/dolly_ignition/models/dolly_ignition/model.sdf b/dolly_ignition/models/dolly_ignition/model.sdf index b707d52..fb7b86f 100644 --- a/dolly_ignition/models/dolly_ignition/model.sdf +++ b/dolly_ignition/models/dolly_ignition/model.sdf @@ -242,7 +242,7 @@ 0 3.45e-06 - 0 0 -0.04 0 -0 0 + 0 0 -0.04 0 -0 0 diff --git a/dolly_tests/CHANGELOG.rst b/dolly_tests/CHANGELOG.rst new file mode 100644 index 0000000..4bf8a63 --- /dev/null +++ b/dolly_tests/CHANGELOG.rst @@ -0,0 +1,4 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package dolly_tests +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + diff --git a/dolly_tests/CMakeLists.txt b/dolly_tests/CMakeLists.txt new file mode 100644 index 0000000..0025204 --- /dev/null +++ b/dolly_tests/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5) + +project(dolly_tests) + +# Fortress +if("$ENV{IGNITION_VERSION}" STREQUAL "fortress") + find_package(ignition-gazebo6 REQUIRED) + set(IGN_GAZEBO_VER 6) + + message(STATUS "Compiling against Ignition Fortress") +# Default to Edifice +else() + find_package(ignition-gazebo5 QUIET) + set(IGN_GAZEBO_VER 5) + + if(NOT ignition-gazebo5_FOUND) + # Skip if Ignition not present + message(WARNING "Ignition Gazebo 5 or 6 not found, proceeding without that simulator.") + return() + else() + message(STATUS "Compiling against Ignition Edifice") + endif() +endif() + +find_package(ament_cmake REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + ament_find_gtest() + + set("PROJECT_BINARY_PATH" ${CMAKE_CURRENT_BINARY_DIR}) + set("PROJECT_SOURCE_PATH" ${CMAKE_CURRENT_SOURCE_DIR}) + configure_file(test/constants.hh.in constants.hh @ONLY) + + set(TEST_NAME follow_ignition_TEST) + ament_add_gtest_executable(${TEST_NAME} test/${TEST_NAME}.cpp) + target_link_libraries(${TEST_NAME} + ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER} + ) + include_directories(${CMAKE_CURRENT_BINARY_DIR}) + install( + TARGETS ${TEST_NAME} + DESTINATION lib/${PROJECT_NAME} + ) + add_launch_test(launch/${TEST_NAME}.launch.py + TIMEOUT 200 + ) +endif() + +install( + DIRECTORY + worlds + DESTINATION + share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/dolly_tests/launch/follow_ignition_TEST.launch.py b/dolly_tests/launch/follow_ignition_TEST.launch.py new file mode 100644 index 0000000..aa0138c --- /dev/null +++ b/dolly_tests/launch/follow_ignition_TEST.launch.py @@ -0,0 +1,92 @@ +# Copyright 2021 Louise Poubel +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest +import launch_testing + +import launch +import launch.actions +import launch_testing.actions +import launch_testing.markers + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + + +def generate_test_description(): + + # Spawn dolly + pkg_dolly_ignition = get_package_share_directory('dolly_ignition') + spawn = Node(package='ros_ign_gazebo', executable='create', + arguments=[ + '-name', 'dolly', + '-z', '0.225', + '-file', os.path.join(pkg_dolly_ignition, 'models', 'dolly_ignition', + 'model.sdf')], + output='screen') + + # Follow node + follow = Node( + package='dolly_follow', + executable='dolly_follow', + output='screen', + remappings=[ + ('cmd_vel', '/dolly/cmd_vel'), + ('laser_scan', '/dolly/laser_scan') + ] + ) + + # Bridge + bridge = Node( + package='ros_ign_bridge', + executable='parameter_bridge', + arguments=['/dolly/cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist', + '/dolly/laser_scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan', + '/dolly/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry'], + output='screen' + ) + + # Test + gazebo_test_fixture = Node( + package='dolly_tests', + executable='follow_ignition_TEST', + output='screen' + ) + + return launch.LaunchDescription([ + gazebo_test_fixture, + spawn, + bridge, + follow, + launch_testing.util.KeepAliveProc(), + launch_testing.actions.ReadyToTest() + ]), locals() + + +class DollyFollowTest(unittest.TestCase): + + def test_termination(self, gazebo_test_fixture, proc_info): + proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=200) + + +@launch_testing.post_shutdown_test() +class DollyFollowTestAfterShutdown(unittest.TestCase): + + def test_exit_code(self, gazebo_test_fixture, proc_info): + launch_testing.asserts.assertExitCodes( + proc_info, + [launch_testing.asserts.EXIT_OK], + gazebo_test_fixture + ) diff --git a/dolly_tests/package.xml b/dolly_tests/package.xml new file mode 100644 index 0000000..4a21f59 --- /dev/null +++ b/dolly_tests/package.xml @@ -0,0 +1,33 @@ + + + + dolly_tests + 0.4.0 + + Tests for the Dolly robot. + + Louise Poubel + Apache 2.0 + + ament_cmake + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + dolly_follow + dolly_ignition + launch_testing + ros2launch + ros_ign_bridge + + + ignition-gazebo6 + + ignition-gazebo5 + ignition-gazebo5 + + + ament_cmake + + + diff --git a/dolly_tests/test/constants.hh.in b/dolly_tests/test/constants.hh.in new file mode 100644 index 0000000..f3c09e7 --- /dev/null +++ b/dolly_tests/test/constants.hh.in @@ -0,0 +1,20 @@ +// Copyright 2021 Louise Poubel. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DOLLY_TESTS__CONSTANTS_HH__ +#define DOLLY_TESTS__CONSTANTS_HH__ +#cmakedefine PROJECT_BINARY_PATH "@PROJECT_BINARY_PATH@" +#cmakedefine PROJECT_SOURCE_PATH "@PROJECT_SOURCE_PATH@" +#endif + diff --git a/dolly_tests/test/follow_ignition_TEST.cpp b/dolly_tests/test/follow_ignition_TEST.cpp new file mode 100644 index 0000000..1a31f2d --- /dev/null +++ b/dolly_tests/test/follow_ignition_TEST.cpp @@ -0,0 +1,223 @@ +// Copyright 2021 Louise Poubel. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "constants.hh" + +using namespace std::chrono_literals; + +////////////////////////////////////////////////// +TEST(DollyTests, Follow) +{ + // Maximum verbosity helps with debugging + ignition::common::Console::SetVerbosity(4); + + // Instantiate test fixture. It starts a simulation server and provides + // hooks that we'll use to inspect the running simulation. + ignition::gazebo::TestFixture fixture(ignition::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "worlds", "empty.sdf")); + + // Variables that will be populated during the simulation + int iterations{0}; + ignition::gazebo::World world; + ignition::gazebo::Entity dollyEntity{ignition::gazebo::kNullEntity}; + std::vector dollyPoses; + ignition::gazebo::Entity targetEntity{ignition::gazebo::kNullEntity}; + + fixture. + // Use configure callback to get values at startup + OnConfigure( + [&](const ignition::gazebo::Entity & _worldEntity, + const std::shared_ptr &, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager &) + { + // Get world + world = ignition::gazebo::World(_worldEntity); + }). + // Use post-update callback to get values at the end of every iteration + OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager & _ecm) + { + iterations++; + + // Get dolly entity once it's spawned + dollyEntity = world.ModelByName(_ecm, "dolly"); + if (ignition::gazebo::kNullEntity == dollyEntity) { + return; + } + + EXPECT_NE(ignition::gazebo::kNullEntity, dollyEntity); + + // Inspect all model poses + dollyPoses.push_back(ignition::gazebo::worldPose(dollyEntity, _ecm)); + + // Get target entity once it's spawned + targetEntity = world.ModelByName(_ecm, "target"); + }). + // The moment we finalize, the configure callback is called + Finalize(); + + // Run simulation server, this will call the post-update callbacks. + int sleep = 0; + int maxSleep = 30; + for (; sleep <= maxSleep && ignition::gazebo::kNullEntity == dollyEntity; + ++sleep) + { + std::this_thread::sleep_for(100ms); + fixture.Server()->Run(true /*blocking*/, 100, false /*paused*/); + } + + EXPECT_LT(sleep, maxSleep); + EXPECT_EQ(100 * sleep, iterations); + EXPECT_NE(ignition::gazebo::kNullEntity, dollyEntity); + EXPECT_LT(0u, dollyPoses.size()); + + // Check that Dolly didn't move, because there's nothing to follow + for (auto i = 0; i < dollyPoses.size(); ++i) { + const auto & pose = dollyPoses[i]; + EXPECT_NEAR(0.0, pose.Pos().X(), 1e-3) << i; + EXPECT_NEAR(0.0, pose.Pos().Y(), 1e-3) << i; + EXPECT_NEAR(0.22, pose.Pos().Z(), 1e-2) << i; + EXPECT_NEAR(0.0, pose.Rot().Roll(), 1e-3) << i; + EXPECT_NEAR(0.0, pose.Rot().Pitch(), 1e-3) << i; + EXPECT_NEAR(0.0, pose.Rot().Yaw(), 1e-3) << i; + } + + // Spawn an object in front of Dolly, to the right + const auto modelStr = std::string("") + + "" + + "" + + "" + + "" + + "1.0" + + "" + + "" + + "1.0" + + "" + + "" + + "" + + ""; + + ignition::msgs::EntityFactory req; + req.set_sdf(modelStr); + + auto pose = req.mutable_pose(); + auto pos = pose->mutable_position(); + pos->set_x(5); + pos->set_y(-3); + + ignition::msgs::Boolean res; + bool result; + unsigned int timeout = 2000; + std::string service{"/world/empty/create"}; + + ignition::transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run simulation until target is spawned + iterations = 0; + sleep = 0; + for (; sleep <= maxSleep && ignition::gazebo::kNullEntity == targetEntity; + ++sleep) + { + std::this_thread::sleep_for(100ms); + fixture.Server()->Run(true, 10, false); + } + + EXPECT_LT(sleep, maxSleep); + EXPECT_EQ(10 * sleep, iterations); + EXPECT_NE(ignition::gazebo::kNullEntity, targetEntity); + EXPECT_LT(0u, dollyPoses.size()); + + // Dolly hasn't moved, because simulation is paused + { + const auto & pose = dollyPoses.back(); + EXPECT_NEAR(0.0, pose.Pos().X(), 1e-3); + EXPECT_NEAR(0.0, pose.Pos().Y(), 1e-3); + EXPECT_NEAR(0.22, pose.Pos().Z(), 1e-2); + EXPECT_NEAR(0.0, pose.Rot().Roll(), 1e-3); + EXPECT_NEAR(0.0, pose.Rot().Pitch(), 1e-3); + EXPECT_NEAR(0.0, pose.Rot().Yaw(), 2e-3); + } + + // Run simulation and check that Dolly moves towards target + iterations = 0; + dollyPoses.clear(); + sleep = 0; + for (; sleep <= maxSleep && dollyPoses.back().Pos().X() < 1.0; ++sleep) { + std::this_thread::sleep_for(100ms); + fixture.Server()->Run(true, 1000, false); + } + + EXPECT_LT(sleep, maxSleep); + EXPECT_EQ(1000 * sleep, iterations); + EXPECT_NE(ignition::gazebo::kNullEntity, targetEntity); + EXPECT_LT(4000u, dollyPoses.size()); + + ignwarn << "Recorded [" << dollyPoses.size() << "] poses" << std::endl; + + for (auto i = 2000; i < dollyPoses.size(); i = i + 100) { + if (i == 2000) { + continue; + } + + const auto & prevPose = dollyPoses[i - 100]; + const auto & pose = dollyPoses[i]; + + // Going forward + EXPECT_LT(prevPose.Pos().X(), pose.Pos().X()) << i; + + // Going right + EXPECT_GT(prevPose.Pos().Y(), pose.Pos().Y()) << i; + + // Turning right + EXPECT_GT(prevPose.Rot().Yaw(), pose.Rot().Yaw()) << i; + + // Not flying, rolling or pitching + EXPECT_NEAR(prevPose.Pos().Z(), pose.Pos().Z(), 1e-3) << i; + EXPECT_NEAR(prevPose.Rot().Roll(), pose.Rot().Roll(), 1e-3) << i; + EXPECT_NEAR(prevPose.Rot().Pitch(), pose.Rot().Pitch(), 1e-3) << i; + } + + { + const auto & pose = dollyPoses.back(); + EXPECT_LT(1.0, pose.Pos().X()); + EXPECT_GT(-0.2, pose.Pos().Y()); + EXPECT_NEAR(0.22, pose.Pos().Z(), 1e-2); + EXPECT_NEAR(0.0, pose.Rot().Roll(), 1e-3); + EXPECT_NEAR(0.0, pose.Rot().Pitch(), 1e-3); + EXPECT_GT(-0.5, pose.Rot().Yaw()); + } +} diff --git a/dolly_tests/worlds/empty.sdf b/dolly_tests/worlds/empty.sdf new file mode 100644 index 0000000..083e64f --- /dev/null +++ b/dolly_tests/worlds/empty.sdf @@ -0,0 +1,44 @@ + + + + + 0.005 + + 0 + + + + + + + + + + + + + ogre2 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + +