Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix use of actors that only has trajectory animation #1947

Merged
merged 4 commits into from
Apr 25, 2023
Merged

Fix use of actors that only has trajectory animation #1947

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3620632
actor working without mesh or skeleton
iche033 Mar 29, 2023
a024baf
add integration
iche033 Mar 29, 2023
b29c8a9
remove todo
iche033 Mar 31, 2023
e73121a
lint
iche033 Mar 31, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
add integration 
Signed-off-by: Ian Chen <ichen@openrobotics.org>
  • Loading branch information
@iche033
iche033 committed Mar 29, 2023
commit a024baf753be9d44140655faf4f60c7d2f7acbc3
6 changes: 5 additions & 1 deletion test/integration/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,8 +1,8 @@
set(TEST_TYPE "INTEGRATION")

set(tests
actor.cc
ackermann_steering_system.cc
actor.cc
air_pressure_system.cc
altimeter_system.cc
apply_joint_force_system.cc
Expand Down Expand Up @@ -86,6 +86,7 @@ endif()

# Tests that require a valid display
set(tests_needing_display
actor_trajectory.cc
camera_sensor_background.cc
camera_sensor_background_from_scene.cc
camera_video_record_system.cc
Expand Down Expand Up @@ -173,4 +174,7 @@ if(VALID_DISPLAY AND VALID_DRI_DISPLAY)
target_link_libraries(INTEGRATION_sensors_system
ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER}
)
target_link_libraries(INTEGRATION_actor_trajectory
ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER}
)
endif()
166 changes: 166 additions & 0 deletions test/integration/actor_trajectory.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,166 @@
/*
* Copyright (C) 2023 Open Source Robotics Foundation
*
* 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 <gtest/gtest.h>

#include <string>
#include <unordered_set>
#include <vector>

#include <ignition/utilities/ExtraTestMacros.hh>

#include <ignition/rendering/RenderingIface.hh>
#include <ignition/rendering/Scene.hh>

#include "ignition/gazebo/EntityComponentManager.hh"
#include "ignition/gazebo/EventManager.hh"
#include "ignition/gazebo/Server.hh"
#include "ignition/gazebo/SystemLoader.hh"
#include "ignition/gazebo/Types.hh"
#include "ignition/gazebo/test_config.hh"

#include "ignition/gazebo/rendering/Events.hh"

#include "plugins/MockSystem.hh"
#include "../helpers/EnvTestFixture.hh"

using namespace ignition;
using namespace std::chrono_literals;

// Pointer to scene
rendering::ScenePtr g_scene;

// Map of model names to their poses
std::unordered_map<std::string, std::vector<math::Pose3d>> g_modelPoses;

// mutex to project model poses
std::mutex g_mutex;

/////////////////////////////////////////////////
void OnPostRender()
{
if (!g_scene)
{
g_scene = rendering::sceneFromFirstRenderEngine();
}
ASSERT_TRUE(g_scene);

auto rootVis = g_scene->RootVisual();
ASSERT_TRUE(rootVis);

// store all the model poses
std::lock_guard<std::mutex> lock(g_mutex);
for (unsigned int i = 0; i < rootVis->ChildCount(); ++i)
{
auto vis = rootVis->ChildByIndex(i);
ASSERT_TRUE(vis);
g_modelPoses[vis->Name()].push_back(vis->WorldPose());
}
}

//////////////////////////////////////////////////
class ActorFixture : public InternalFixture<InternalFixture<::testing::Test>>
{
protected: void SetUp() override
{
InternalFixture::SetUp();

sdf::Plugin sdfPlugin;
sdfPlugin.SetFilename("libMockSystem.so");
sdfPlugin.SetName("ignition::gazebo::MockSystem");
auto plugin = sm.LoadPlugin(sdfPlugin);
EXPECT_TRUE(plugin.has_value());
this->systemPtr = plugin.value();
this->mockSystem = static_cast<gazebo::MockSystem *>(
systemPtr->QueryInterface<gazebo::System>());
}

public: gazebo::SystemPluginPtr systemPtr;
public: gazebo::MockSystem *mockSystem;

private: gazebo::SystemLoader sm;
};

/////////////////////////////////////////////////
// Load the actor_trajectory.sdf world that animates a box (actor) to follow
// a trajectory. Verify that the box pose changes over time on the rendering
// side.
TEST_F(ActorFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(ActorTrajectoryNoMesh))
{
gazebo::ServerConfig serverConfig;

const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) +
"/test/worlds/actor_trajectory.sdf";

serverConfig.SetSdfFile(sdfFile);
gazebo::Server server(serverConfig);

common::ConnectionPtr postRenderConn;

// A pointer to the ecm. This will be valid once we run the mock system
gazebo::EntityComponentManager *ecm = nullptr;
this->mockSystem->preUpdateCallback =
[&ecm](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm)
{
ecm = &_ecm;
};
this->mockSystem->configureCallback =
[&](const gazebo::Entity &,
const std::shared_ptr<const sdf::Element> &,
gazebo::EntityComponentManager &,
gazebo::EventManager &_eventMgr)
{
postRenderConn = _eventMgr.Connect<gazebo::events::PostRender>(
std::bind(&::OnPostRender));
};

server.AddSystem(this->systemPtr);
server.Run(true, 500, false);
ASSERT_NE(nullptr, ecm);

// verify that pose of the animated box exists
bool hasBoxPose = false;
int sleep = 0;
int maxSleep = 50;
const std::string boxName = "animated_box";
unsigned int boxPoseCount = 0u;
while (!hasBoxPose && sleep++ < maxSleep)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
std::lock_guard<std::mutex> lock(g_mutex);
if (g_modelPoses.find(boxName) != g_modelPoses.end())
{
hasBoxPose = true;
boxPoseCount = g_modelPoses.size();
}
}
EXPECT_TRUE(hasBoxPose);
EXPECT_LT(1u, boxPoseCount);

// check that box is animated, i.e. pose changes over time
{
std::lock_guard<std::mutex> lock(g_mutex);
auto it = g_modelPoses.find(boxName);
auto &poses = it->second;
for (unsigned int i = 0; i < poses.size()-1; ++i)
{
EXPECT_NE(poses[i], poses[i+1]);
}
}

g_scene.reset();
}
83 changes: 83 additions & 0 deletions test/worlds/actor_trajectory.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,83 @@
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="actors">

<!-- add sensors system for rendering to run on server -->
<plugin
filename="ignition-gazebo-sensors-system"
name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>

<!-- box actor with no skin mesh but follows a trajectory -->
<actor name="animated_box">
<link name="box_link">
<visual name="visual">
<geometry>
<box>
<size>.2 .2 .2</size>
</box>
</geometry>
</visual>
</link>
<script>
<loop>true</loop>
<auto_start>true</auto_start>
<trajectory id="0" type="square">
<waypoint>
<time>0.0</time>
<pose>-1 -1 1 0 0 0</pose>
</waypoint>
<waypoint>
<time>1.0</time>
<pose>-1 1 1 0 0 0</pose>
</waypoint>
<waypoint>
<time>2.0</time>
<pose>1 1 1 0 0 0</pose>
</waypoint>
<waypoint>
<time>3.0</time>
<pose>1 -1 1 0 0 0</pose>
</waypoint>
<waypoint>
<time>4.0</time>
<pose>-1 -1 1 0 0 0</pose>
</waypoint>
</trajectory>
</script>
</actor>

<!-- add camera or any rendering sensor for rendering to run on server -->
<model name="camera">
<static>true</static>
<pose>-5 0 0 0 0.5 0</pose>
<link name="link">
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="camera" type="camera">
<pose>0 0 0.0 0 0 0</pose>
<camera>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<update_rate>30</update_rate>
<topic>camera</topic>
</sensor>
</link>
</model>

</world>
</sdf>