From 5689318d4ee193a3e789c64874cf724d527b05e2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Sat, 2 May 2020 13:54:11 -0500 Subject: [PATCH 1/4] Add PerformerDetector, a system for detecting when performers enter a specified region Signed-off-by: Addisu Z. Taddese --- examples/worlds/performer_detector.sdf | 494 ++++++++++++++++++ src/LevelManager.cc | 16 +- src/systems/CMakeLists.txt | 1 + src/systems/performer_detector/CMakeLists.txt | 6 + .../performer_detector/PerformerDetector.cc | 208 ++++++++ .../performer_detector/PerformerDetector.hh | 106 ++++ test/integration/CMakeLists.txt | 1 + test/integration/performer_detector.cc | 141 +++++ test/worlds/performer_detector.sdf | 361 +++++++++++++ 9 files changed, 1326 insertions(+), 8 deletions(-) create mode 100644 examples/worlds/performer_detector.sdf create mode 100644 src/systems/performer_detector/CMakeLists.txt create mode 100644 src/systems/performer_detector/PerformerDetector.cc create mode 100644 src/systems/performer_detector/PerformerDetector.hh create mode 100644 test/integration/performer_detector.cc create mode 100644 test/worlds/performer_detector.sdf diff --git a/examples/worlds/performer_detector.sdf b/examples/worlds/performer_detector.sdf new file mode 100644 index 0000000000..60baf937dc --- /dev/null +++ b/examples/worlds/performer_detector.sdf @@ -0,0 +1,494 @@ + + + + + + + + + 0.001 + 1.0 + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/performer_detector/control + /world/performer_detector/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/performer_detector/stats + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + true + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 1.25 + 0.3 + + + + + true + 10 0 2.5 0 0 0 + + + 0.9 + + + 10 10 5 + + + + 0.0 1.0 0.0 1 + 0.0 1.0 0.0 1 + 0.5 0.5 0.5 1 + + false + + + + /performer_detector + + + 10 10 5 + + + + + + true + 10 5 2.5 0 0 0 + + + 0.9 + + + 10 10 5 + + + + 0.0 1.0 0.0 1 + 0.0 1.0 0.0 1 + 0.5 0.5 0.5 1 + + false + + + + /performer_detector + + + 10 10 5 + + + + + + + + + + vehicle_blue + + + 2 2 2 + + + + + + + + + diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 4589cb6553..0df0e1ed92 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -132,19 +132,19 @@ void LevelManager::ReadLevelPerformerInfo() } } - if (this->useLevels) + if (pluginElem == nullptr) { - if (pluginElem == nullptr) + if (this->useLevels) { ignerr << "Could not find a plugin tag with name " << kPluginName << ". Levels and distributed simulation will not work.\n"; } - else - { - this->ReadPerformers(pluginElem); - if (this->useLevels) - this->ReadLevels(pluginElem); - } + } + else + { + this->ReadPerformers(pluginElem); + if (this->useLevels) + this->ReadLevels(pluginElem); } this->ConfigureDefaultLevel(); diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index c1f37d15eb..87f6d11e8d 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -88,6 +88,7 @@ add_subdirectory(logical_camera) add_subdirectory(magnetometer) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) +add_subdirectory(performer_detector) add_subdirectory(physics) add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) diff --git a/src/systems/performer_detector/CMakeLists.txt b/src/systems/performer_detector/CMakeLists.txt new file mode 100644 index 0000000000..41bf315618 --- /dev/null +++ b/src/systems/performer_detector/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(performer-detector + SOURCES + PerformerDetector.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/performer_detector/PerformerDetector.cc b/src/systems/performer_detector/PerformerDetector.cc new file mode 100644 index 0000000000..acb15391fb --- /dev/null +++ b/src/systems/performer_detector/PerformerDetector.cc @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2020 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 + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/components/Geometry.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Performer.hh" +#include "ignition/gazebo/components/Pose.hh" + +#include "PerformerDetector.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +///////////////////////////////////////////////// +void PerformerDetector::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->model = Model(_entity); + if (!this->model.Valid(_ecm)) + { + ignerr << "PerformerDetector should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + this->detectorName = this->model.Name(_ecm); + + auto modelPose = _ecm.Component(_entity)->Data(); + + auto sdfClone = _sdf->Clone(); + bool hasGeometry{false}; + if (sdfClone->HasElement("geometry")) + { + auto geom = sdfClone->GetElement("geometry"); + if (geom->HasElement("box")) + { + auto box = geom->GetElement("box"); + auto boxSize = box->Get("size"); + this->detectorGeometry = math::AxisAlignedBox(-boxSize / 2, boxSize / 2); + hasGeometry = true; + } + } + + if (!hasGeometry) + { + ignerr << "'' is a required parameter for " + "PerformerDetector. Failed to initialize.\n"; + return; + } + + std::string defaultTopic{"/model/" + this->model.Name(_ecm) + + "/performer_detector/status"}; + auto topic = _sdf->Get("topic", defaultTopic).first; + + ignmsg << "PerformerDetector publishing messages on " + << "[" << topic << "]" << std::endl; + + transport::Node node; + this->pub = node.Advertise(topic); + this->initialized = true; +} + +////////////////////////////////////////////////// +void PerformerDetector::PostUpdate( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("PerformerDetector::PostUpdate"); + + if (_info.paused) + return; + + if (this->initialized) + { + auto modelPose = + _ecm.Component(this->model.Entity())->Data(); + + // Double negative because AxisAlignedBox does not currently have operator+ + // that takes a position + auto region = this->detectorGeometry - (-modelPose.Pos()); + + _ecm.Each( + [&](const Entity &_entity, const components::Performer *, + const components::Geometry *_geometry, + const components::ParentEntity *_parent) -> bool + { + auto pose = _ecm.Component(_parent->Data())->Data(); + auto name = _ecm.Component(_parent->Data())->Data(); + const math::Pose3d relPose = modelPose.Inverse() * pose; + + // We assume the geometry contains a box. + auto perfBox = _geometry->Data().BoxShape(); + if (nullptr == perfBox) + { + ignerr << "Internal error: geometry of performer [" << _entity + << "] missing box." << std::endl; + return true; + } + + math::AxisAlignedBox performerVolume{ + pose.Pos() - perfBox->Size() / 2, + pose.Pos() + perfBox->Size() / 2}; + + bool alreadyDetected = this->IsAlreadyDetected(_entity); + if (region.Intersects(performerVolume)) + { + if (!alreadyDetected) + { + this->AddToDetected(_entity); + this->Publish(_entity, name, true, relPose, _info.simTime); + } + } + else if (alreadyDetected) + { + this->RemoveFromDetected(_entity); + this->Publish(_entity, name, false, relPose, _info.simTime); + } + + return true; + }); + } +} + +////////////////////////////////////////////////// +bool PerformerDetector::IsAlreadyDetected(const Entity &_entity) const +{ + return this->detectedEntities.find(_entity) != this->detectedEntities.end(); +} + +////////////////////////////////////////////////// +void PerformerDetector::AddToDetected(const Entity &_entity) +{ + this->detectedEntities.insert(_entity); +} + +////////////////////////////////////////////////// +void PerformerDetector::RemoveFromDetected(const Entity &_entity) +{ + this->detectedEntities.erase(_entity); +} + +////////////////////////////////////////////////// +void PerformerDetector::Publish( + const Entity &_entity, const std::string &_name, bool _state, + const math::Pose3d &_pose, + const std::chrono::steady_clock::duration &_stamp) +{ + msgs::Pose msg = msgs::Convert(_pose); + msg.set_name(_name); + msg.set_id(_entity); + + auto stamp = math::durationToSecNsec(_stamp); + msg.mutable_header()->mutable_stamp()->set_sec(stamp.first); + msg.mutable_header()->mutable_stamp()->set_nsec(stamp.second); + + { + auto *headerData = msg.mutable_header()->add_data(); + headerData->set_key("frame_id"); + headerData->add_value(this->detectorName); + } + { + auto *headerData = msg.mutable_header()->add_data(); + headerData->set_key("state"); + headerData->add_value(std::to_string(_state)); + } + + this->pub.Publish(msg); +} + +IGNITION_ADD_PLUGIN(PerformerDetector, + ignition::gazebo::System, + PerformerDetector::ISystemConfigure, + PerformerDetector::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(PerformerDetector, + "ignition::gazebo::systems::PerformerDetector") diff --git a/src/systems/performer_detector/PerformerDetector.hh b/src/systems/performer_detector/PerformerDetector.hh new file mode 100644 index 0000000000..a9dfd38313 --- /dev/null +++ b/src/systems/performer_detector/PerformerDetector.hh @@ -0,0 +1,106 @@ +/* + * Copyright (C) 2020 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. + * + */ + +#ifndef IGNITION_GAZEBO_SYSTEMS_PERFORMERDETECTOR_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PERFORMERDETECTOR_HH_ + +#include +#include +#include + +#include + +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + + class IGNITION_GAZEBO_VISIBLE PerformerDetector + : public System, + public ISystemConfigure, + public ISystemPostUpdate + { + /// Documentation inherited + public: PerformerDetector() = default; + + /// Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) final; + + /// Documentation inherited + public: void PostUpdate( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) final; + + /// \brief Check if the entity has already been detected + /// \param [in] _entity The entity to test + /// \returns True if the entity has already been detected + private: bool IsAlreadyDetected(const Entity &_entity) const; + + /// \brief Add the entity to the list of detected entities + /// \param [in] _entity The entity to add + private: void AddToDetected(const Entity &_entity); + + /// \brief Remove the entity from the list of detected entities + /// \param [in] _entity The entity to remove + private: void RemoveFromDetected(const Entity &_entity); + + /// \brief Publish the event that the entity is detected or no longer + /// detected. + /// \param [in] _entity The entity to report + /// \param [in] _name The name of the entity that triggered the event + /// \param [in] _state The new state of the detector + /// \param [in] _pose The pose of the entity that triggered the event + /// \param [in] _stamp Time stamp of the event + private: void Publish(const Entity &_entity, const std::string &_name, + bool _state, const math::Pose3d &_pose, + const std::chrono::steady_clock::duration &_stamp); + + /// \brief Keeps a set of detected entities + private: std::unordered_set detectedEntities; + + /// \brief The model associated with this system. + private: Model model; + + /// \brief Name of the detector used as the frame_id in published messages. + private: std::string detectorName; + + /// \brief Detector region. Only a box geometry is supported + private: math::AxisAlignedBox detectorGeometry; + + /// \brief Ignition communication publisher. + private: transport::Node::Publisher pub; + + /// \brief Whether the system has been initialized + private: bool initialized{false}; + }; + + } +} +} +} + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 37ea5a0b47..8b13f15799 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -25,6 +25,7 @@ set(tests model.cc multicopter.cc network_handshake.cc + performer_detector.cc physics_system.cc play_pause.cc pose_publisher_system.cc diff --git a/test/integration/performer_detector.cc b/test/integration/performer_detector.cc new file mode 100644 index 0000000000..83274ccc7a --- /dev/null +++ b/test/integration/performer_detector.cc @@ -0,0 +1,141 @@ +/* + * Copyright (C) 2020 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 +#include + +#include + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "plugins/MockSystem.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +class PerformerDetectorTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } + + protected: std::unique_ptr StartServer(const std::string &_filePath) + { + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + _filePath; + serverConfig.SetSdfFile(sdfFile); + + auto server = std::make_unique(serverConfig); + EXPECT_FALSE(server->Running()); + EXPECT_FALSE(*server->Running(0)); + + using namespace std::chrono_literals; + server->SetUpdatePeriod(1ns); + return server; + } + + protected: std::mutex poseMsgsMutex; + protected: std::vector poseMsgs; +}; + +///////////////////////////////////////////////// +// Test that commanded motor speed is applied +TEST_F(PerformerDetectorTest, MovingPerformer) +{ + auto server = this->StartServer("/test/worlds/performer_detector.sdf"); + + transport::Node node; + auto cmdVelPub = node.Advertise("/model/vehicle_blue/cmd_vel"); + + auto detectorCb = std::function( + [this](const auto &_msg) + { + std::lock_guard lock(this->poseMsgsMutex); + this->poseMsgs.push_back(_msg); + }); + + node.Subscribe("/performer_detector", detectorCb); + + server->Run(true, 1, false); + msgs::Twist cmdVelMsg; + cmdVelMsg.mutable_linear()->set_x(2.0); + cmdVelPub.Publish(cmdVelMsg); + + const std::size_t nIters{6000}; + server->Run(true, nIters, false); + + // Wait for messages to arrive in poseMsgs or a timeout is reached + const auto timeOut = 5s; + auto tInit = std::chrono::steady_clock::now(); + auto tNow = tInit; + while (tNow - tInit < timeOut) + { + std::this_thread::sleep_for(100ms); + + std::lock_guard lock(this->poseMsgsMutex); + if (this->poseMsgs.size() == 4) + break; + + tNow = std::chrono::steady_clock::now(); + } + + ASSERT_EQ(4u, this->poseMsgs.size()); + EXPECT_EQ("detector1", this->poseMsgs[0].header().data(0).value(0)); + EXPECT_EQ("1", this->poseMsgs[0].header().data(1).value(0)); + EXPECT_EQ("detector2", this->poseMsgs[1].header().data(0).value(0)); + EXPECT_EQ("1", this->poseMsgs[1].header().data(1).value(0)); + EXPECT_EQ("detector1", this->poseMsgs[2].header().data(0).value(0)); + EXPECT_EQ("0", this->poseMsgs[2].header().data(1).value(0)); + EXPECT_EQ("detector2", this->poseMsgs[3].header().data(0).value(0)); + EXPECT_EQ("0", this->poseMsgs[3].header().data(1).value(0)); + + // The performer's bounding box is 2x2. It starts at a position of {0, 2} and + // moves straight in the +x direction. The performer enters the detector's + // region when the its bounding box interesects with the detector's region. + // The reported position is relative to the detector. + + // detector1's XY position is {4, 0} with a region of 4x4. Accounting for the + // performer's own bounding box, the interval of interesection becomes: + // x:[4 - 2 - 1, 4 + 2 + 1], y: [0 - 2 - 1, 0 + 2 + 1] + // = x:[1, 7], y:[-3, 3] + // The position of the performer is {1, 2} when it enters detector1's region + // and {7, 2} when it leaves the region + // The reported position is relative to the detector. + EXPECT_NEAR(-3.0, this->poseMsgs[0].position().x(), 1e-2); + EXPECT_NEAR(2.0, this->poseMsgs[0].position().y(), 1e-2); + EXPECT_NEAR(3.0, this->poseMsgs[2].position().x(), 1e-2); + EXPECT_NEAR(2.0, this->poseMsgs[2].position().y(), 1e-2); + + // detector2's XY position is {5, 3} with a region of 3x2.5. Accounting for + // the performer's own bounding box, the interval of interesection becomes: + // x:[5 - 1.5 - 1, 5 + 1.5 + 1], y: [3 - 1.25 - 1, 3 + 1.25 + 1] + // = x:[2.5, 7.5], y:[1.75, 5.25] + // The position of the performer is {2.5, 2} when it enters detector2's region + // and {7.5, 2} when it leaves the region + // The reported position is relative to the detector. + EXPECT_NEAR(-2.5, this->poseMsgs[1].position().x(), 1e-2); + EXPECT_NEAR(-1, this->poseMsgs[1].position().y(), 1e-2); + EXPECT_NEAR(2.5, this->poseMsgs[3].position().x(), 1e-2); + EXPECT_NEAR(-1, this->poseMsgs[3].position().y(), 1e-2); +} diff --git a/test/worlds/performer_detector.sdf b/test/worlds/performer_detector.sdf new file mode 100644 index 0000000000..a68f371dce --- /dev/null +++ b/test/worlds/performer_detector.sdf @@ -0,0 +1,361 @@ + + + + + + + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 2 0.325 0 -0 0 + true + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 1.25 + 0.3 + + + + + true + 4.0 0 2.5 0 0 0 + + + /performer_detector + + + 4 4 5 + + + + + + true + 5 3 2.5 0 0 0 + + + /performer_detector + + + 3 2.5 5 + + + + + + + + + vehicle_blue + + + 2 2 2 + + + + + + + + From c15c3fb2955dd6b383e5dfdd0ae516efe02e6648 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 26 May 2020 14:40:43 -0500 Subject: [PATCH 2/4] Update changelog Signed-off-by: Addisu Z. Taddese --- Changelog.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Changelog.md b/Changelog.md index 5b4b1bc9e0..a912f7399d 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,10 @@ ## Ignition Gazebo 2.x +### Ignition Gazebo 2.XX.XX (20XX-XX-XX) + +1. Add PerformerDetector, a system for detecting when performers enter a specified region + * [Pull Request 125](https://github.com/ignitionrobotics/ign-gazebo/pull/125) + ### Ignition Gazebo 2.18.0 (2020-05-20) 1. Added a `/world//create_multiple` service that parallels the current `/world//create` service. The `create_multiple` service can handle an `ignition::msgs::EntityFactory_V` message that may contain one or more entities to spawn. From 5d4efb6bdd8cc7f4de86d99a52e3d5f0b6a83e93 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 28 May 2020 17:52:53 -0500 Subject: [PATCH 3/4] Add system documentation Signed-off-by: Addisu Z. Taddese --- .../performer_detector/PerformerDetector.hh | 32 +++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/src/systems/performer_detector/PerformerDetector.hh b/src/systems/performer_detector/PerformerDetector.hh index a9dfd38313..ea189403d1 100644 --- a/src/systems/performer_detector/PerformerDetector.hh +++ b/src/systems/performer_detector/PerformerDetector.hh @@ -35,6 +35,38 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { + /// \brief A system system that publishes on a topic when a performer enters + /// or leaves a specified region. + /// + /// A performer is detected when a performer's volume, which is + /// represented by an ignition::math::AxisAlignedBox, intersects with the + /// PerformerDetector's region, which is also represented by an + /// ignition::math::AxisAlignedBox. When a performer is detected, the system + /// publishes an ignition.msgs.Pose message with the pose of the detected + /// performer with respect to the model containing the PerformerDetector. The + /// name and id fields of the Pose message will be set to the name and the + /// entity of the detected performer respectively. The header of the Pose + /// message contains the time stamp of detection. The `data` field of the + /// header will contain the key "frame_id" with a value set to the name of + /// the model containing the PerformerDetector system and the key "state" with + /// a value set to "1" if the performer is entering the detector's region and + /// "0" if the performer is leaving the region. + /// + /// The PerformerDetector has to be attached to a and it's region is + /// centered on the containing model's origin. + /// + /// The system does not assume that levels are enabled, but it does require + /// performers to be specified. + /// + /// ## System parameters + /// + /// ``: Custom topic to be used for publishing when a performer is + /// detected. If not set, the default topic with the following pattern would + /// be used "/model//performer_detector/status". The topic type + /// is ignition.msgs.Pose + /// ``: Detection region. Currently, only the `` geometry is + /// supported. The position of the geometry is derived from the pose of the + /// containing model. class IGNITION_GAZEBO_VISIBLE PerformerDetector : public System, From 728969a2fc588a2cf834988ddf456e7618a298f8 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 28 May 2020 18:01:53 -0500 Subject: [PATCH 4/4] Address reviewere feedback Signed-off-by: Addisu Z. Taddese --- examples/worlds/performer_detector.sdf | 4 + .../performer_detector/PerformerDetector.cc | 89 +++++++++---------- 2 files changed, 48 insertions(+), 45 deletions(-) diff --git a/examples/worlds/performer_detector.sdf b/examples/worlds/performer_detector.sdf index 60baf937dc..0eef4f5428 100644 --- a/examples/worlds/performer_detector.sdf +++ b/examples/worlds/performer_detector.sdf @@ -6,6 +6,10 @@ To move the vehicle: ign topic -t "/model/vehicle_blue/cmd_vel" -m ignition.msgs.Twist -p "linear: {x: 0.5}" + + Then, echo the PerformerDetector's topic to see when the vehicle is detected + + ign topic -e -t /performer_detector --> diff --git a/src/systems/performer_detector/PerformerDetector.cc b/src/systems/performer_detector/PerformerDetector.cc index acb15391fb..70a899f088 100644 --- a/src/systems/performer_detector/PerformerDetector.cc +++ b/src/systems/performer_detector/PerformerDetector.cc @@ -56,8 +56,6 @@ void PerformerDetector::Configure(const Entity &_entity, this->detectorName = this->model.Name(_ecm); - auto modelPose = _ecm.Component(_entity)->Data(); - auto sdfClone = _sdf->Clone(); bool hasGeometry{false}; if (sdfClone->HasElement("geometry")) @@ -101,56 +99,57 @@ void PerformerDetector::PostUpdate( if (_info.paused) return; - if (this->initialized) + if (!this->initialized) { - auto modelPose = - _ecm.Component(this->model.Entity())->Data(); - - // Double negative because AxisAlignedBox does not currently have operator+ - // that takes a position - auto region = this->detectorGeometry - (-modelPose.Pos()); - - _ecm.Each( - [&](const Entity &_entity, const components::Performer *, - const components::Geometry *_geometry, - const components::ParentEntity *_parent) -> bool - { - auto pose = _ecm.Component(_parent->Data())->Data(); - auto name = _ecm.Component(_parent->Data())->Data(); - const math::Pose3d relPose = modelPose.Inverse() * pose; + return; + } - // We assume the geometry contains a box. - auto perfBox = _geometry->Data().BoxShape(); - if (nullptr == perfBox) - { - ignerr << "Internal error: geometry of performer [" << _entity - << "] missing box." << std::endl; - return true; - } + auto modelPose = + _ecm.Component(this->model.Entity())->Data(); + + // Double negative because AxisAlignedBox does not currently have operator+ + // that takes a position + auto region = this->detectorGeometry - (-modelPose.Pos()); + + _ecm.Each( + [&](const Entity &_entity, const components::Performer *, + const components::Geometry *_geometry, + const components::ParentEntity *_parent) -> bool + { + auto pose = _ecm.Component(_parent->Data())->Data(); + auto name = _ecm.Component(_parent->Data())->Data(); + const math::Pose3d relPose = modelPose.Inverse() * pose; + + // We assume the geometry contains a box. + auto perfBox = _geometry->Data().BoxShape(); + if (nullptr == perfBox) + { + ignerr << "Internal error: geometry of performer [" << _entity + << "] missing box." << std::endl; + return true; + } - math::AxisAlignedBox performerVolume{ - pose.Pos() - perfBox->Size() / 2, - pose.Pos() + perfBox->Size() / 2}; + math::AxisAlignedBox performerVolume{pose.Pos() - perfBox->Size() / 2, + pose.Pos() + perfBox->Size() / 2}; - bool alreadyDetected = this->IsAlreadyDetected(_entity); - if (region.Intersects(performerVolume)) - { - if (!alreadyDetected) - { - this->AddToDetected(_entity); - this->Publish(_entity, name, true, relPose, _info.simTime); - } - } - else if (alreadyDetected) + bool alreadyDetected = this->IsAlreadyDetected(_entity); + if (region.Intersects(performerVolume)) + { + if (!alreadyDetected) { - this->RemoveFromDetected(_entity); - this->Publish(_entity, name, false, relPose, _info.simTime); + this->AddToDetected(_entity); + this->Publish(_entity, name, true, relPose, _info.simTime); } + } + else if (alreadyDetected) + { + this->RemoveFromDetected(_entity); + this->Publish(_entity, name, false, relPose, _info.simTime); + } - return true; - }); - } + return true; + }); } //////////////////////////////////////////////////