From f66ca1eae236b8c95ed67e0176be86b963ff7457 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 8 May 2020 10:27:14 -0500 Subject: [PATCH 01/23] Add TriggeredPublisher system Signed-off-by: Addisu Z. Taddese --- examples/worlds/triggered_publisher.sdf | 425 +++++++++++++++ src/systems/CMakeLists.txt | 1 + .../triggered_publisher/CMakeLists.txt | 7 + .../triggered_publisher/TriggeredPublisher.cc | 496 ++++++++++++++++++ .../triggered_publisher/TriggeredPublisher.hh | 178 +++++++ test/integration/CMakeLists.txt | 1 + test/integration/triggered_publisher.cc | 381 ++++++++++++++ test/worlds/triggered_publisher.sdf | 118 +++++ 8 files changed, 1607 insertions(+) create mode 100644 examples/worlds/triggered_publisher.sdf create mode 100644 src/systems/triggered_publisher/CMakeLists.txt create mode 100644 src/systems/triggered_publisher/TriggeredPublisher.cc create mode 100644 src/systems/triggered_publisher/TriggeredPublisher.hh create mode 100644 test/integration/triggered_publisher.cc create mode 100644 test/worlds/triggered_publisher.sdf diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf new file mode 100644 index 0000000000..569e9ef217 --- /dev/null +++ b/examples/worlds/triggered_publisher.sdf @@ -0,0 +1,425 @@ + + + + + + 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/triggered_publisher/control + /world/triggered_publisher/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/triggered_publisher/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 + + + -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 + cmd_vel + + + + + data: "true" + + + + linear: {x: 1.5} + + + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index c1f37d15eb..36531893c4 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -93,5 +93,6 @@ add_subdirectory(pose_publisher) add_subdirectory(scene_broadcaster) add_subdirectory(sensors) add_subdirectory(touch_plugin) +add_subdirectory(triggered_publisher) add_subdirectory(user_commands) add_subdirectory(wind_effects) diff --git a/src/systems/triggered_publisher/CMakeLists.txt b/src/systems/triggered_publisher/CMakeLists.txt new file mode 100644 index 0000000000..55ba451d01 --- /dev/null +++ b/src/systems/triggered_publisher/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(triggered-publisher + SOURCES + TriggeredPublisher.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc new file mode 100644 index 0000000000..9f69673e57 --- /dev/null +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -0,0 +1,496 @@ +/* + * 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 "TriggeredPublisher.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Base class for input matchers. +class systems::InputMatcher +{ + /// \brief Match input message against the match criteria. + /// \param[in] _input Input message + /// \return True if the input matches the match criteria. + public: virtual bool Match(const transport::ProtoMsg &_input) const = 0; + + /// \brief Destructor + public: virtual ~InputMatcher() = default; + + /// \brief Checks if the matcher is in a valid state. + /// \return True if the matcher is in a valid state. + public: virtual bool IsValid() const; + + /// \brief Factory function for creating matchers. + /// \param[in] _msgType Input message type (eg. ignition.msgs.Boolean) + /// \param[in] _matchElem the SDFormat Element that contains the configuration + /// for the matcher + /// \return A concrete InputMatcher initialized according to the contents of + /// _matchElem. A nullptr is returned if the created InputMatcher is invalid. + public: static std::unique_ptr Create( + const std::string &_msgType, const sdf::ElementPtr &_matchElem); + + /// \brief State of the matcher + protected: bool valid{false}; +}; + +////////////////////////////////////////////////// +/// \brief Matches any input message +class AnyMatcher : public InputMatcher +{ + // Documentation inherited + public: bool Match(const transport::ProtoMsg &_input) const override; +}; + + +////////////////////////////////////////////////// +/// \brief Matches the whole input message against the match criteria. Floats +/// are compared using MathUtil::AlmostEquals() +class FullMatcher : public InputMatcher +{ + /// \brief Constructor + /// \param[in] _msgType Input message type + /// \param[in] _logicType Determines what the returned value of Match() would + /// be on a successful comparison. If this is false, a successful match would + /// return false. + /// \param[in] _matchString String used to construct the protobuf message + /// against which input messages are matched. This is the human-readable + /// representation of a protobuf message as used by `ign topic` for pulishing + /// messages + public: FullMatcher(const std::string &_msgType, bool _logicType, + const std::string &_matchString); + + // Documentation inherited + public: bool Match(const transport::ProtoMsg &_input) const override; + + /// \brief Protobuf message for matching against input + protected: std::unique_ptr matchMsg; + + /// \brief Logic type of this matcher + protected: const bool logicType; +}; + +////////////////////////////////////////////////// +/// \brief Matches a specific field in the input message against the match +/// criteria. Floats are compared using MathUtil::AlmostEquals() +class FieldMatcher : public InputMatcher +{ + /// \brief Constructor + /// \param[in] _msgType Input message type + /// \param[in] _logicType Determines what the returned value of Match() would + /// be on a successful comparison. If this is false, a successful match would + /// return false. + /// \param[in] _fieldName Name of the field to compare + /// \param[in] _fieldString String used to construct the protobuf message + /// against which the specified field in the input messages are matched. This + /// is the human-readable representation of a protobuf message as used by `ign + /// topic` for pulishing messages + public: FieldMatcher(const std::string &_msgType, bool _logicType, + const std::string &_fieldName, + const std::string &_fieldString); + + // Documentation inherited + public: bool Match(const transport::ProtoMsg &_input) const override; + protected: static bool FindFieldSubMessage( + transport::ProtoMsg *_msg, const std::string &_fieldName, + std::vector + &_fieldDesc, + transport::ProtoMsg **_subMsg); + + /// \brief Protobuf message for matching against input + protected: std::unique_ptr matchMsg; + + /// \brief Logic type of this matcher + protected: const bool logicType; + + /// \brief Name of the field compared by this matcher + protected: const std::string fieldName; + + /// \brief Field descriptor of the field compared by this matcher + protected: std::vector + fieldDescMatcher; +}; + +////////////////////////////////////////////////// +bool AnyMatcher::Match(const transport::ProtoMsg &) const +{ + return true; +} + +////////////////////////////////////////////////// +FullMatcher::FullMatcher(const std::string &_msgType, bool _logicType, + const std::string &_matchString) + : matchMsg(msgs::Factory::New(_msgType, _matchString)), + logicType(_logicType) +{ + if (nullptr != matchMsg) + this->valid = true; +} + +////////////////////////////////////////////////// +bool FullMatcher::Match( + const transport::ProtoMsg &_input) const +{ + return this->logicType == + google::protobuf::util::MessageDifferencer::ApproximatelyEquals( + *this->matchMsg, _input); +} +////////////////////////////////////////////////// +FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, + const std::string &_fieldName, + const std::string &_fieldString) + : matchMsg(msgs::Factory::New(_msgType)), + logicType(_logicType), + fieldName(_fieldName) +{ + if (nullptr == this->matchMsg) + { + return; + } + + transport::ProtoMsg *matcherSubMsg{nullptr}; + FindFieldSubMessage(this->matchMsg.get(), _fieldName, this->fieldDescMatcher, + &matcherSubMsg); + + if (this->fieldDescMatcher.empty()) + return; + if (nullptr == matcherSubMsg) + return; + + try + { + bool result = google::protobuf::TextFormat::ParseFieldValueFromString( + _fieldString, this->fieldDescMatcher.back(), matcherSubMsg); + if (!result) + { + ignerr << "Failed to parse matcher string [" + << _fieldString << "] for field [" << this->fieldName + << "] of input message type [" << _msgType << "]\n"; + return; + } + } + catch (const std::exception &_err) + { + ignerr << "Creating Field matcher failed: " << _err.what() << std::endl; + return; + } + + this->valid = true; +} +////////////////////////////////////////////////// +bool FieldMatcher::FindFieldSubMessage( + transport::ProtoMsg *_msg, const std::string &_fieldName, + std::vector &_fieldDesc, + transport::ProtoMsg **_subMsg) +{ + const google::protobuf::Descriptor *fieldMsgType = _msg->GetDescriptor(); + + // If fieldMsgType is nullptr, then this is not a composite message and we + // shouldn't be using a FieldMatcher + if (nullptr == fieldMsgType) + return false; + + *_subMsg = _msg; + + auto fieldNames = common::split(_fieldName, "."); + for (std::size_t i = 0; i < fieldNames.size(); ++i) + { + auto fieldDesc = fieldMsgType->FindFieldByName(fieldNames[i]); + + if (nullptr == fieldDesc) + break; + + _fieldDesc.push_back(fieldDesc); + + if (google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE != + fieldDesc->cpp_type()) + { + break; + } + fieldMsgType = fieldDesc->message_type(); + + if (nullptr == fieldMsgType) + break; + + if (i < fieldNames.size() - 1) + { + auto *reflection = (*_subMsg)->GetReflection(); + if (fieldDesc->is_repeated()) + { + *_subMsg = reflection->AddMessage(*_subMsg, fieldDesc); + } + else + { + *_subMsg = reflection->MutableMessage(*_subMsg, fieldDesc); + } + } + } + + return true; +} + +////////////////////////////////////////////////// +bool FieldMatcher::Match( + const transport::ProtoMsg &_input) const +{ + google::protobuf::util::DefaultFieldComparator comp; + + auto *reflection = this->matchMsg->GetReflection(); + const transport::ProtoMsg *subMsgMatcher = this->matchMsg.get(); + const transport::ProtoMsg *subMsgInput = &_input; + for (std::size_t i = 0; i < this->fieldDescMatcher.size() - 1; ++i) + { + auto *fieldDesc = this->fieldDescMatcher[i]; + if (fieldDesc->is_repeated()) + { + subMsgMatcher = + &reflection->GetRepeatedMessage(*subMsgMatcher, fieldDesc, 0); + subMsgInput = + &reflection->GetRepeatedMessage(*subMsgInput, fieldDesc, 0); + } + else + { + subMsgMatcher = &reflection->GetMessage(*subMsgMatcher, fieldDesc); + subMsgInput = &reflection->GetMessage(*subMsgInput, fieldDesc); + } + } + + google::protobuf::util::MessageDifferencer diff; + diff.set_scope(google::protobuf::util::MessageDifferencer::PARTIAL); + diff.set_repeated_field_comparison( + google::protobuf::util::MessageDifferencer::AS_SET); + return this->logicType == + diff.CompareWithFields(*subMsgMatcher, *subMsgInput, + {this->fieldDescMatcher.back()}, + {this->fieldDescMatcher.back()}); +} + +////////////////////////////////////////////////// +bool InputMatcher::IsValid() const +{ + return this->valid; +} + +////////////////////////////////////////////////// +std::unique_ptr InputMatcher::Create( + const std::string &_msgType, const sdf::ElementPtr &_matchElem) +{ + if (nullptr == _matchElem) + { + return std::make_unique(); + } + + std::unique_ptr matcher{nullptr}; + + const auto logicType = _matchElem->Get("logic_type", true).first; + auto inputMatchString = common::trimmed(_matchElem->Get()); + if (!inputMatchString.empty()) + { + if (_matchElem->HasAttribute("field")) + { + const auto fieldName = _matchElem->Get("field"); + matcher = std::make_unique(_msgType, logicType, fieldName, + inputMatchString); + } + else + { + matcher = + std::make_unique(_msgType, logicType, inputMatchString); + } + if (matcher == nullptr || !matcher->IsValid()) + { + ignerr << "Matcher type could not be created from:\n" + << inputMatchString << std::endl; + return nullptr; + } + } + return matcher; +} + + +////////////////////////////////////////////////// +TriggeredPublisher::~TriggeredPublisher() +{ + this->done = true; + this->newMatchSignal.notify_one(); + if (this->workerThread.joinable()) + { + this->workerThread.join(); + } +} +////////////////////////////////////////////////// +void TriggeredPublisher::Configure(const Entity &, + const std::shared_ptr &_sdf, + EntityComponentManager &, + EventManager &) +{ + sdf::ElementPtr sdfClone = _sdf->Clone(); + if (sdfClone->HasElement("input")) + { + auto inputElem = sdfClone->GetElement("input"); + this->inputMsgType = inputElem->Get("type"); + this->inputTopic = inputElem->Get("topic"); + + if (inputElem->HasElement("match")) + { + for (auto matchElem = inputElem->GetElement("match"); matchElem; + matchElem = matchElem->GetNextElement("match")) + { + auto matcher = InputMatcher::Create(this->inputMsgType, matchElem); + if (nullptr != matcher) + { + this->matchers.push_back(std::move(matcher)); + } + } + } + else + { + auto matcher = InputMatcher::Create(this->inputMsgType, nullptr); + if (nullptr != matcher) + { + this->matchers.push_back(std::move(matcher)); + } + } + } + else + { + ignerr << "No input specified" << std::endl; + return; + } + + if (this->matchers.empty()) + { + ignerr << "No valid matchers specified\n"; + return; + } + + if (sdfClone->HasElement("output")) + { + for (auto outputElem = sdfClone->GetElement("output"); outputElem; + outputElem = outputElem->GetNextElement("output")) + { + OutputInfo info; + info.msgType = outputElem->Get("type"); + info.topic = outputElem->Get("topic"); + if (info.topic.empty()) + { + ignerr << "Topic cannot be empty\n"; + continue; + } + info.msgData = + msgs::Factory::New(info.msgType, outputElem->Get()); + if (nullptr != info.msgData) + { + info.pub = this->node.Advertise(info.topic, info.msgType); + this->outputInfo.push_back(std::move(info)); + } + else + { + ignerr << "Unable to create message of type[" << info.msgType + << "] with data[" << info.msgData << "].\n"; + } + } + } + else + { + ignerr << "No input specified" << std::endl; + return; + } + + auto msgCb = std::function( + [this](const auto &_msg) + { + if (this->MatchInput(_msg)) + { + { + std::lock_guard lock(this->publishCountMutex); + ++publishCount; + } + this->newMatchSignal.notify_one(); + } + }); + this->node.Subscribe(this->inputTopic, msgCb); + + std::stringstream ss; + ss << "TriggeredPublisher subscribed on " << this->inputTopic + << " and publishing on "; + + for (const auto &info : this->outputInfo) + { + ss << info.topic << ", "; + } + igndbg << ss.str() << "\n"; + + this->workerThread = + std::thread(std::bind(&TriggeredPublisher::DoWork, this)); +} + +////////////////////////////////////////////////// +void TriggeredPublisher::DoWork() +{ + while (!this->done) + { + std::size_t pending{0}; + { + using namespace std::chrono_literals; + std::unique_lock lock(this->publishCountMutex); + this->newMatchSignal.wait_for(lock, 1s, + [this] + { + return (this->publishCount > 0) || this->done; + }); + + if (this->publishCount == 0 || this->done) + continue; + + std::swap(pending, this->publishCount); + } + + for (auto &info : this->outputInfo) + { + for (std::size_t i = 0; i < pending; ++i) + { + info.pub.Publish(*info.msgData); + } + } + } +} + +////////////////////////////////////////////////// +bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) +{ + return std::all_of(this->matchers.begin(), this->matchers.end(), + [&](const auto &matcher) + { + return matcher->Match(_inputMsg); + }); +} + +IGNITION_ADD_PLUGIN(TriggeredPublisher, + ignition::gazebo::System, + TriggeredPublisher::ISystemConfigure) + +IGNITION_ADD_PLUGIN_ALIAS(TriggeredPublisher, + "ignition::gazebo::systems::TriggeredPublisher") diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh new file mode 100644 index 0000000000..357ec65695 --- /dev/null +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -0,0 +1,178 @@ +/* + * 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_TRIGGEREDPUBLISHER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_TRIGGEREDPUBLISHER_HH_ + +#include +#include +#include + +#include +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class InputMatcher; + + /// \brief The triggered publisher system publishes a user specified message + /// on an output topic in response to an input message that matches user + /// specified criteria. + /// + /// System Parameters + /// + /// `` The tag contains the input message type, topic and matcher + /// information. + /// * Attributes: + /// * `type`: Input message type (eg. ignition.msgs.Boolean) + /// * `topic`: Input message topic name + /// + /// ``: Contains configuration for matchers. Multiple + /// tags are possible. An output message is triggered if all Matchers match. + /// * Attributes: + /// * `logic_type`("positive" or "negative"): Specifies whether a + /// comparison must succeed or fail in order to trigger an output + /// message. A "positive" value triggers a match when a comparison + /// succeeds. A "negative" value triggers a match when a comparson + /// fails. + /// * `field`: If specified, specified, only this field inside the input + /// message is compared for a match. + /// * Value: String used to construct the protobuf message against which + /// input messages are matched. This is the human-readable + /// representation of a protobuf message as used by `ign topic` for + /// pulishing messages + /// + /// ``: Contains configuration for output messages: Multiple + /// tags are possible. A message will be published on each output topic for + /// each input that matches. + /// * Attributes: + /// * `type`: Output message type (eg. ignition.msgs.Boolean) + /// * `topic`: Output message topic name + /// * Value: String used to construct the output protobuf message . This is + /// the human-readable representation of a protobuf message as used by + /// `ign topic` for pulishing messages + /// + /// Examples: + /// 1. Any receipt of a Boolean messages on the input topic triggers an event + /// + /// + /// + /// + /// + /// 2. Exact match: An output is triggered when a Boolean message with a value + /// of "true" is received + /// + /// + /// + /// data: true + /// + /// + /// + /// + /// + /// 3. Field match: An output is triggered when a specific field matches + /// + /// + /// 1.0 + /// 2.0 + /// + /// + /// + class IGNITION_GAZEBO_VISIBLE TriggeredPublisher : public System, + public ISystemConfigure + { + /// \brief Constructor + public: TriggeredPublisher() = default; + + /// \brief Destructor + public: ~TriggeredPublisher(); + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + /// \brief Thread that handles publishing output messages + public: void DoWork(); + + /// \brief Helper function that calls Match on every InputMatcher available + /// \param[in] _inputMsg Input message + /// \return True if all of the matchers return true + public: bool MatchInput(const transport::ProtoMsg &_inputMsg); + + /// \brief Input message type (eg. ignition.msgs.Boolean) + private: std::string inputMsgType; + + /// \brief Input message topic + private: std::string inputTopic; + + /// \brief Class that holds necessary bits for each specified output. + private: struct OutputInfo + { + /// \brief Output message type + std::string msgType; + + /// \brief Output message topic + std::string topic; + + /// \brief Protobuf message of the output parsed from the human-readable + /// string specified in the element of the plugin's configuration + transport::ProtoMsgPtr msgData; + + /// \brief Transport publisher + transport::Node::Publisher pub; + }; + + /// \brief List of InputMatchers + private: std::vector> matchers; + + /// \brief List of outputs + private: std::vector outputInfo; + + /// \brief Ignition communication node. + private: transport::Node node; + + /// \brief Counter that tells the publisher how many times to publish + private: std::size_t publishCount{0}; + + /// \brief Mutex to synchronize access to publishCount + private: std::mutex publishCountMutex; + + /// \brief Condition variable to signal that new matches have occured + private: std::condition_variable newMatchSignal; + + /// \brief Thread handle for worker thread + private: std::thread workerThread; + + /// \brief Flag for when the system is done and the worker thread should + /// stop + private: std::atomic done{false}; + }; + } +} +} +} + +#endif + diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 37ea5a0b47..a9792bbbc3 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -32,6 +32,7 @@ set(tests scene_broadcaster_system.cc sdf_include.cc touch_plugin.cc + triggered_publisher.cc user_commands.cc log_system.cc wind_effects.cc diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc new file mode 100644 index 0000000000..ac230a8598 --- /dev/null +++ b/test/integration/triggered_publisher.cc @@ -0,0 +1,381 @@ +/* + * 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/test_config.hh" + +#include "plugins/MockSystem.hh" + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +class TriggeredPublisherTest : 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); + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/triggered_publisher.sdf"; + serverConfig.SetSdfFile(sdfFile); + + this->server = std::make_unique(serverConfig); + EXPECT_FALSE(server->Running()); + EXPECT_FALSE(*server->Running(0)); + + server->SetUpdatePeriod(100us); + server->Run(true, 1, false); + } + public: std::unique_ptr server; +}; + + +///////////////////////////////////////////////// +/// Check that empty message types do not need any data to be specified in the +/// configuration +TEST_F(TriggeredPublisherTest, EmptyInputEmptyOutput) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_0"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_0", msgCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + inputPub.Publish(msgs::Empty()); + server->Run(true, 10, false); + } + + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, InputMessagesTriggerOutputs) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_1"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_1", msgCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + inputPub.Publish(msgs::Empty()); + server->Run(true, 10, false); + } + + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, MultipleOutputsForOneInput) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_2"); + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [](std::vector &_msgVector) + { + return std::function( + [&_msgVector](const msgs::Boolean &_msg) + { + _msgVector.push_back(_msg.data()); + }); + }; + + auto msgCb0 = cbCreator(recvMsgs0); + auto msgCb1 = cbCreator(recvMsgs1); + node.Subscribe("/out_2_0", msgCb0); + node.Subscribe("/out_2_1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + inputPub.Publish(msgs::Empty()); + server->Run(true, 10, false); + } + + // The plugin has two outputs. We expect 10 messages in each output topic + EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), false)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, ExactMatchBooleanInputs) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_3"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_3", msgCb); + + const std::size_t pubCount{10}; + const std::size_t trueCount{5}; + for (std::size_t i = 0; i < pubCount; ++i) + { + if (i < trueCount) + { + inputPub.Publish(msgs::Convert(true)); + } + else + { + inputPub.Publish(msgs::Convert(false)); + } + server->Run(true, 100, false); + } + + // The matcher filters out false messages and the inputs consist of 5 true and + // 5 false messages, so we expect 5 output messages + EXPECT_EQ(trueCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, MatcherWithNegativeLogicType) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_4"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_4", msgCb); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + inputPub.Publish(msgs::Convert(static_cast(i - pubCount / 2))); + server->Run(true, 100, false); + } + // The matcher filters out 0 so we expect 9 output messages from the 10 inputs + EXPECT_EQ(9u, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, MultipleMatchersAreAnded) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_5"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_5", msgCb); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + inputPub.Publish(msgs::Convert(static_cast(i - pubCount / 2))); + server->Run(true, 100, false); + } + // The matcher filters out negative numbers and the input is [-5,4], so we + // expect 5 output messages. + EXPECT_EQ(5u, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, FieldMatchers) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_6"); + std::size_t recvCount[2]{0, 0}; + + auto cbCreator = [](std::size_t &_counter) + { + return std::function( + [&_counter](const msgs::Empty &) + { + ++_counter; + }); + }; + + auto msgCb0 = cbCreator(recvCount[0]); + auto msgCb1 = cbCreator(recvCount[1]); + node.Subscribe("/out_6_0", msgCb0); + node.Subscribe("/out_6_1", msgCb1); + + const int pubCount{10}; + msgs::Vector2d msg; + msg.set_x(1.0); + for (int i = 0; i < pubCount; ++i) + { + msg.set_y(static_cast(i)); + inputPub.Publish(msg); + server->Run(true, 100, false); + } + + // The first plugin matches x==1 and y==2 which only once out of the 10 inputs + EXPECT_EQ(1u, recvCount[0]); + // The second plugin matches x==1 and y!=2 which occurs 9 out of the 10 inputs + EXPECT_EQ(9u, recvCount[1]); +} + +///////////////////////////////////////////////// +/// Tests that if the specified field is a repeated field, a partial match is +/// used when comparing against the input. +TEST_F(TriggeredPublisherTest, FieldMatchersWithRepeatedFieldsUsePartialMatches) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_7"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_7", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + msgs::Pose poseMsg; + auto *frame = poseMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(std::string("frame") + std::to_string(i)); + + auto *time = poseMsg.mutable_header()->mutable_stamp(); + time->set_sec(10); + + auto *other = poseMsg.mutable_header()->add_data(); + other->set_key("other_key"); + other->add_value("other_value"); + inputPub.Publish(poseMsg); + server->Run(true, 100, false); + } + + // The matcher filters out frame ids that are not frame0, so we expect 1 + // output. Even though the data field contains other key-value pairs, since + // repeated fields use partial matching, the matcher will match one of the + // inputs. + EXPECT_EQ(1u, recvCount); +} + +///////////////////////////////////////////////// +/// Tests that field matchers can be used to do a full match with repeated +/// fields by specifying the containing field of the repeated field in the +/// "field" attribute and setting the desired values of the repeated field in +/// the value of the tag. +TEST_F(TriggeredPublisherTest, + FieldMatchersWithRepeatedFieldsInValueUseFullMatches) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_7"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_7", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + msgs::Pose poseMsg; + auto *frame = poseMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(std::string("frame") + std::to_string(i)); + + auto *time = poseMsg.mutable_header()->mutable_stamp(); + time->set_sec(10); + + auto *other = poseMsg.mutable_header()->add_data(); + other->set_key("other_key"); + other->add_value("other_value"); + inputPub.Publish(poseMsg); + server->Run(true, 100, false); + } + + // The matcher filters out frame ids that are not frame0, so we expect 1 + // output. Even though the data field contains other key-value pairs, since + // repeated fields use partial matching, the matcher will match one of the + // inputs. + EXPECT_EQ(1u, recvCount); +} + +///////////////////////////////////////////////// +/// Tests that full matchers can be used with repeated fields by specifying the +/// desired values of the repeated field in the value of the tag. The +/// message created from the value of must be a full match of the input. +TEST_F(TriggeredPublisherTest, + FullMatchersWithRepeatedFieldsInValueUseFullMatches) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_9"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_9", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + msgs::Int32_V msg; + for (int i = 0; i < pubCount; ++i) + { + msg.add_data(i); + inputPub.Publish(msg); + server->Run(true, 100, false); + } + + // The input contains an increasing sets of sequences, {0}, {0,1}, {0,1,2}... + // The matcher only matches {0,1} + EXPECT_EQ(1u, recvCount); +} diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf new file mode 100644 index 0000000000..19ce5cb1d6 --- /dev/null +++ b/test/worlds/triggered_publisher.sdf @@ -0,0 +1,118 @@ + + + + + + + + + + + + data: true + + + + + + + data: false + + + data: true + + + + + + + data: true + + + + + + + + + data: 0 + + + + + + + + + data: -5 + + + data: -4 + + + data: -3 + + + data: -2 + + + data: -1 + + + + + + + + 1.0 + 2.0 + + + + + + + 1.0 + 2.0 + + + + + + + + { + key: "frame_id" + value: "frame0" + } + + + + + + + + { + data { + key: "frame_id" + value: "frame0" + } + data { + key: "other_key" + value: "other_value" + } + } + + + + + + + data: 0, data: 1 + + + + + + + From 31f46e3b45a702edc4f6436dfb869bb9a0897f70 Mon Sep 17 00:00:00 2001 From: John Date: Thu, 14 May 2020 10:19:20 -0700 Subject: [PATCH 02/23] Add shapes plugin (#107) Signed-off-by: John Shepherd Signed-off-by: Addisu Z. Taddese --- include/ignition/gazebo/gui/GuiEvents.hh | 22 ++ .../ignition/gazebo/rendering/SceneManager.hh | 4 + src/gui/plugins/CMakeLists.txt | 1 + src/gui/plugins/scene3d/GzScene3D.qml | 3 + src/gui/plugins/scene3d/Scene3D.cc | 250 ++++++++++++++++++ src/gui/plugins/scene3d/Scene3D.hh | 44 +++ src/gui/plugins/shapes/CMakeLists.txt | 4 + src/gui/plugins/shapes/Shapes.cc | 207 +++++++++++++++ src/gui/plugins/shapes/Shapes.hh | 57 ++++ src/gui/plugins/shapes/Shapes.qml | 88 ++++++ src/gui/plugins/shapes/Shapes.qrc | 8 + src/gui/plugins/shapes/box.png | Bin 0 -> 863 bytes src/gui/plugins/shapes/cylinder.png | Bin 0 -> 983 bytes src/gui/plugins/shapes/sphere.png | Bin 0 -> 1449 bytes src/rendering/SceneManager.cc | 6 + 15 files changed, 694 insertions(+) create mode 100644 src/gui/plugins/shapes/CMakeLists.txt create mode 100644 src/gui/plugins/shapes/Shapes.cc create mode 100644 src/gui/plugins/shapes/Shapes.hh create mode 100644 src/gui/plugins/shapes/Shapes.qml create mode 100644 src/gui/plugins/shapes/Shapes.qrc create mode 100644 src/gui/plugins/shapes/box.png create mode 100644 src/gui/plugins/shapes/cylinder.png create mode 100644 src/gui/plugins/shapes/sphere.png diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index da28caa77f..524805dd89 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -18,6 +18,7 @@ #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_ #include +#include #include #include #include @@ -159,6 +160,27 @@ namespace events /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 3); }; + + /// \brief Event called to spawn a preview model. Used in shapes plugin. + class SpawnPreviewModel : public QEvent + { + public: explicit SpawnPreviewModel(std::string &_modelSdfString) + : QEvent(kType), modelSdfString(_modelSdfString) + { + } + /// \brief Unique type for this event. + static const QEvent::Type kType = QEvent::Type(QEvent::User + 4); + + /// \brief Get the sdf string of the model. + /// \return The model sdf string + public: std::string ModelSdfString() const + { + return this->modelSdfString; + } + + /// \brief The sdf string of the model to be previewed. + std::string modelSdfString; + }; } // namespace events } } // namespace gui diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 74dcba935c..27d3f7fdac 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -64,6 +64,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _id World ID. public: void SetWorldId(Entity _id); + /// \brief Get the world's ID. + /// \return World ID + public: Entity WorldId() const; + /// \brief Create a model /// \param[in] _id Unique model id /// \param[in] _model Model sdf dom diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 0594c2c183..a1d479e215 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -85,6 +85,7 @@ add_subdirectory(component_inspector) add_subdirectory(entity_tree) add_subdirectory(grid_config) add_subdirectory(scene3d) +add_subdirectory(shapes) add_subdirectory(transform_control) add_subdirectory(video_recorder) add_subdirectory(view_angle) diff --git a/src/gui/plugins/scene3d/GzScene3D.qml b/src/gui/plugins/scene3d/GzScene3D.qml index aeec5ce7fc..1f8289465f 100644 --- a/src/gui/plugins/scene3d/GzScene3D.qml +++ b/src/gui/plugins/scene3d/GzScene3D.qml @@ -40,6 +40,9 @@ Rectangle { onEntered: { GzScene3D.OnFocusWindow() } + onPositionChanged: { + GzScene3D.OnHovered(mouseArea.mouseX, mouseArea.mouseY); + } } RenderWindow { diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 5b66af2838..c29b501c3d 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -21,11 +21,17 @@ #include #include +#include +#include +#include +#include + #include #include #include #include #include +#include #include #include @@ -136,6 +142,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Flag to indicate if mouse event is dirty public: bool mouseDirty = false; + /// \brief Flag to indicate if hover event is dirty + public: bool hoverDirty = false; + /// \brief Mouse event public: common::MouseEvent mouseEvent; @@ -206,6 +215,31 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Flag for indicating whether we are in view angle mode or not public: bool viewAngle = false; + /// \brief Flag for indicating whether we are in shapes mode or not + public: bool spawnModel = false; + + /// \brief Flag for indicating whether the user is currently placing a + /// model with the shapes plugin or not + public: bool placingModel = false; + + /// \brief The sdf string of the model to be used with the shapes plugin + public: std::string modelSdfString; + + /// \brief The pose of the preview model + public: ignition::math::Pose3d previewModelPose = + ignition::math::Pose3d::Zero; + + /// \brief The currently hovered mouse position in screen coordinates + public: math::Vector2i mouseHoverPos = math::Vector2i::Zero; + + /// \brief The model generated from the modelSdfString upon the user + /// clicking a shape in the shapes plugin + public: rendering::VisualPtr spawnPreviewModel = nullptr; + + /// \brief A record of the ids currently used by the model generation + /// for easy deletion of visuals later + public: std::vector modelIds; + /// \brief The pose set during a view angle button press that holds /// the pose the camera should assume relative to the entit(y/ies). /// The vector (0, 0, 0) indicates to return the camera back to the home @@ -236,6 +270,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Name of service for setting entity pose public: std::string poseCmdService; + /// \brief Name of service for creating entity + public: std::string createCmdService; + /// \brief The starting world pose of a clicked visual. public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; @@ -309,6 +346,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Follow service public: std::string viewAngleService; + + /// \brief Shapes service + public: std::string shapesService; }; } } @@ -551,6 +591,19 @@ void IgnRenderer::Render() } } + // Shapes + { + IGN_PROFILE("IgnRenderer::Render Shapes"); + if (this->dataPtr->spawnModel) + { + rendering::ScenePtr scene = this->dataPtr->renderUtil.Scene(); + rendering::VisualPtr rootVis = scene->RootVisual(); + this->dataPtr->placingModel = + this->GeneratePreviewModel(this->dataPtr->modelSdfString); + this->dataPtr->spawnModel = false; + } + } + if (ignition::gui::App()) { ignition::gui::App()->sendEvent( @@ -559,11 +612,93 @@ void IgnRenderer::Render() } } +///////////////////////////////////////////////// +bool IgnRenderer::GeneratePreviewModel(const std::string &_modelSdfString) +{ + sdf::Root root; + root.LoadSdfString(_modelSdfString); + + if (!root.ModelCount()) + { + this->TerminatePreviewModel(); + return false; + } + + // Only preview first model + sdf::Model model = *(root.ModelByIndex(0)); + this->dataPtr->previewModelPose = model.Pose(); + model.SetName(ignition::common::Uuid().String()); + Entity modelId = this->UniqueId(); + if (!modelId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->spawnPreviewModel = + this->dataPtr->renderUtil.SceneManager().CreateModel( + modelId, model, + this->dataPtr->renderUtil.SceneManager().WorldId()); + + this->dataPtr->modelIds.push_back(modelId); + for (auto j = 0u; j < model.LinkCount(); j++) + { + sdf::Link link = *(model.LinkByIndex(j)); + link.SetName(ignition::common::Uuid().String()); + Entity linkId = this->UniqueId(); + if (!linkId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->renderUtil.SceneManager().CreateLink( + linkId, link, modelId); + this->dataPtr->modelIds.push_back(linkId); + for (auto k = 0u; k < link.VisualCount(); k++) + { + sdf::Visual visual = *(link.VisualByIndex(k)); + visual.SetName(ignition::common::Uuid().String()); + Entity visualId = this->UniqueId(); + if (!visualId) + { + this->TerminatePreviewModel(); + return false; + } + this->dataPtr->renderUtil.SceneManager().CreateVisual( + visualId, visual, linkId); + this->dataPtr->modelIds.push_back(visualId); + } + } + return true; +} + +///////////////////////////////////////////////// +void IgnRenderer::TerminatePreviewModel() +{ + for (auto _id : this->dataPtr->modelIds) + this->dataPtr->renderUtil.SceneManager().RemoveEntity(_id); + this->dataPtr->modelIds.clear(); + this->dataPtr->placingModel = false; +} + +///////////////////////////////////////////////// +Entity IgnRenderer::UniqueId() +{ + auto timeout = 100000u; + for (auto i = 0u; i < timeout; ++i) + { + Entity id = std::numeric_limits::max() - i; + if (!this->dataPtr->renderUtil.SceneManager().HasEntity(id)) + return id; + } + return kNullEntity; +} + ///////////////////////////////////////////////// void IgnRenderer::HandleMouseEvent() { std::lock_guard lock(this->dataPtr->mutex); this->HandleMouseContextMenu(); + this->HandleModelPlacement(); this->HandleMouseTransformControl(); this->HandleMouseViewControl(); } @@ -707,6 +842,51 @@ void IgnRenderer::HandleKeyRelease(QKeyEvent *_e) } } +///////////////////////////////////////////////// +void IgnRenderer::HandleModelPlacement() +{ + if (!this->dataPtr->placingModel) + return; + + if (this->dataPtr->spawnPreviewModel && this->dataPtr->hoverDirty) + { + math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseHoverPos); + pos.Z(this->dataPtr->spawnPreviewModel->WorldPosition().Z()); + this->dataPtr->spawnPreviewModel->SetWorldPosition(pos); + this->dataPtr->hoverDirty = false; + } + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && + this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE && + !this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty) + { + // Delete the generated visuals + this->TerminatePreviewModel(); + + math::Pose3d modelPose = this->dataPtr->previewModelPose; + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error creating model" << std::endl; + }; + math::Vector3d pos = this->ScreenToPlane(this->dataPtr->mouseEvent.Pos()); + pos.Z(modelPose.Pos().Z()); + msgs::EntityFactory req; + req.set_sdf(this->dataPtr->modelSdfString); + req.set_allow_renaming(true); + msgs::Set(req.mutable_pose(), math::Pose3d(pos, modelPose.Rot())); + + if (this->dataPtr->createCmdService.empty()) + { + this->dataPtr->createCmdService = "/world/" + this->worldName + + "/create"; + } + this->dataPtr->node.Request(this->dataPtr->createCmdService, req, cb); + this->dataPtr->placingModel = false; + this->dataPtr->mouseDirty = false; + } +} + ///////////////////////////////////////////////// void IgnRenderer::HandleEntitySelection() { @@ -1352,6 +1532,14 @@ void IgnRenderer::SetTransformMode(const std::string &_mode) } } +///////////////////////////////////////////////// +void IgnRenderer::SetModel(const std::string &_model) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->spawnModel = true; + this->dataPtr->modelSdfString = _model; +} + ///////////////////////////////////////////////// void IgnRenderer::SetRecordVideo(bool _record, const std::string &_format, const std::string &_savePath) @@ -1449,6 +1637,14 @@ void IgnRenderer::OnViewAngleComplete() this->dataPtr->viewAngle = false; } +///////////////////////////////////////////////// +void IgnRenderer::NewHoverEvent(const math::Vector2i &_hoverPos) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->mouseHoverPos = _hoverPos; + this->dataPtr->hoverDirty = true; +} + ///////////////////////////////////////////////// void IgnRenderer::NewMouseEvent(const common::MouseEvent &_e, const math::Vector2d &_drag) @@ -1459,6 +1655,29 @@ void IgnRenderer::NewMouseEvent(const common::MouseEvent &_e, this->dataPtr->mouseDirty = true; } +///////////////////////////////////////////////// +math::Vector3d IgnRenderer::ScreenToPlane( + const math::Vector2i &_screenPos) const +{ + // Normalize point on the image + double width = this->dataPtr->camera->ImageWidth(); + double height = this->dataPtr->camera->ImageHeight(); + + double nx = 2.0 * _screenPos.X() / width - 1.0; + double ny = 1.0 - 2.0 * _screenPos.Y() / height; + + // Make a ray query + this->dataPtr->rayQuery->SetFromCamera( + this->dataPtr->camera, math::Vector2d(nx, ny)); + + ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), 0); + + math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + math::Vector3d direction = this->dataPtr->rayQuery->Direction(); + double distance = plane.Distance(origin, direction); + return origin + direction * distance; +} + ///////////////////////////////////////////////// math::Vector3d IgnRenderer::ScreenToScene( const math::Vector2i &_screenPos) const @@ -2018,6 +2237,13 @@ bool Scene3D::OnViewAngle(const msgs::Vector3d &_msg, return true; } +///////////////////////////////////////////////// +void Scene3D::OnHovered(int _mouseX, int _mouseY) +{ + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->OnHovered({_mouseX, _mouseY}); +} + ///////////////////////////////////////////////// void Scene3D::OnDropped(const QString &_drop, int _mouseX, int _mouseY) { @@ -2132,6 +2358,17 @@ bool Scene3D::eventFilter(QObject *_obj, QEvent *_event) renderWindow->SetScaleSnap(snapEvent->Scale()); } } + else if (_event->type() == + ignition::gazebo::gui::events::SpawnPreviewModel::kType) + { + auto spawnPreviewModelEvent = + reinterpret_cast(_event); + if (spawnPreviewModelEvent) + { + auto renderWindow = this->PluginItem()->findChild(); + renderWindow->SetModel(spawnPreviewModelEvent->ModelSdfString()); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); @@ -2151,6 +2388,12 @@ void RenderWindowItem::SetTransformMode(const std::string &_mode) this->dataPtr->renderThread->ignRenderer.SetTransformMode(_mode); } +///////////////////////////////////////////////// +void RenderWindowItem::SetModel(const std::string &_model) +{ + this->dataPtr->renderThread->ignRenderer.SetModel(_model); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetRecordVideo(bool _record, const std::string &_format, const std::string &_savePath) @@ -2224,6 +2467,12 @@ void RenderWindowItem::SetWorldName(const std::string &_name) this->dataPtr->renderThread->ignRenderer.worldName = _name; } +///////////////////////////////////////////////// +void RenderWindowItem::OnHovered(const ignition::math::Vector2i &_hoverPos) +{ + this->dataPtr->renderThread->ignRenderer.NewHoverEvent(_hoverPos); +} + ///////////////////////////////////////////////// void RenderWindowItem::mousePressEvent(QMouseEvent *_e) { @@ -2307,6 +2556,7 @@ void RenderWindowItem::keyReleaseEvent(QKeyEvent *_e) _e->accept(); } this->DeselectAllEntities(true); + this->dataPtr->renderThread->ignRenderer.TerminatePreviewModel(); } } diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index f6f47335c2..2ae15cbe32 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -96,6 +96,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public slots: void OnDropped(const QString &_drop, int _mouseX, int _mouseY); + /// \brief Callback when the mouse hovers to a new position. + /// \param[in] _mouseX x coordinate of the hovered mouse position. + /// \param[in] _mouseY y coordinate of the hovered mouse position. + public slots: void OnHovered(int _mouseX, int _mouseY); + /// \brief Callback when the mouse enters the render window to /// focus the window for mouse/key events public slots: void OnFocusWindow(); @@ -175,6 +180,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _mode New transform mode to set to public: void SetTransformMode(const std::string &_mode); + /// \brief Set the model to hover over the scene. + /// \param[in] _model Sdf string of the model to load in for the user. + public: void SetModel(const std::string &_model); + /// \brief Set whether to record video /// \param[in] _record True to start video recording, false to stop. /// \param[in] _format Video encoding format: "mp4", "ogv" @@ -235,6 +244,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: void NewMouseEvent(const common::MouseEvent &_e, const math::Vector2d &_drag = math::Vector2d::Zero); + /// \brief New hover event triggered. + /// \param[in] _hoverPos Mouse hover screen position + public: void NewHoverEvent(const math::Vector2i &_hoverPos); + /// \brief Handle key press event for snapping /// \param[in] _e The key event to process. public: void HandleKeyPress(QKeyEvent *_e); @@ -316,6 +329,28 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Handle entity selection requests private: void HandleEntitySelection(); + /// \brief Handle model placement requests + private: void HandleModelPlacement(); + + /// \brief Generate a unique entity id. + /// \return The unique entity id + private: Entity UniqueId(); + + /// \brief Generate a preview of a model. + /// \param[in] _modelSdfString The sdf string of the model to be generated + /// \return True on success, false if failure + public: bool GeneratePreviewModel(const std::string &_modelSdfString); + + /// \brief Delete the visuals generated from the shapes plugin. + public: void TerminatePreviewModel(); + + /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a + /// ray cast from the given 2D screen coordinates. + /// \param[in] _screenPod 2D coordinates on the screen, in pixels. + /// \return 3D coordinates of a point in the 3D scene. + public: math::Vector3d ScreenToPlane(const math::Vector2i &_screenPos) + const; + /// \brief Retrieve the first point on a surface in the 3D scene hit by a /// ray cast from the given 2D screen coordinates. /// \param[in] _screenPos 2D coordinates on the screen, in pixels. @@ -435,6 +470,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _mode New transform mode to set to public: void SetTransformMode(const std::string &_mode); + /// \brief Set the model to hover. + /// \param[in] _model Sdf string of the model to load in for the user. + public: void SetModel(const std::string &_model); + /// \brief Set whether to record video /// \param[in] _record True to start video recording, false to stop. /// \param[in] _format Video encoding format: "mp4", "ogv" @@ -514,6 +553,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return 3D coordinates of a point in the 3D scene. public: math::Vector3d ScreenToScene(const math::Vector2i &_screenPos); + /// \brief Called when the mouse hovers to a new position. + /// \param[in] _hoverPos 2D coordinates of the hovered mouse position on + /// the render window. + public: void OnHovered(const ignition::math::Vector2i &_hoverPos); + /// \brief Slot called when thread is ready to be started public Q_SLOTS: void Ready(); diff --git a/src/gui/plugins/shapes/CMakeLists.txt b/src/gui/plugins/shapes/CMakeLists.txt new file mode 100644 index 0000000000..fd6c2d7458 --- /dev/null +++ b/src/gui/plugins/shapes/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_gui_plugin(Shapes + SOURCES Shapes.cc + QT_HEADERS Shapes.hh +) diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc new file mode 100644 index 0000000000..7aa4eeea13 --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.cc @@ -0,0 +1,207 @@ +/* + * 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/EntityComponentManager.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" + +#include "Shapes.hh" + +namespace ignition::gazebo +{ + class ShapesPrivate + { + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Mutex to protect mode + public: std::mutex mutex; + + /// \brief Transform control service name + public: std::string service; + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +Shapes::Shapes() + : ignition::gui::Plugin(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +Shapes::~Shapes() = default; + +///////////////////////////////////////////////// +void Shapes::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Shapes"; + + // For shapes requests + ignition::gui::App()->findChild + ()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void Shapes::OnMode(const QString &_mode) +{ + std::string modelSdfString = _mode.toStdString(); + std::transform(modelSdfString.begin(), modelSdfString.end(), + modelSdfString.begin(), ::tolower); + + if (modelSdfString == "box") + { + modelSdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "0.167" + "0" + "0" + "0.167" + "0" + "0.167" + "" + "1.0" + "" + "" + "" + "" + "1 1 1" + "" + "" + "" + "" + "" + "" + "1 1 1" + "" + "" + "" + "" + "" + ""); + } + else if (modelSdfString == "sphere") + { + modelSdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "0.1" + "0" + "0" + "0.1" + "0" + "0.1" + "" + "1.0" + "" + "" + "" + "" + "0.5" + "" + "" + "" + "" + "" + "" + "0.5" + "" + "" + "" + "" + "" + ""); + } + else if (modelSdfString == "cylinder") + { + modelSdfString = std::string("" + "" + "" + "0 0 0.5 0 0 0" + "" + "" + "" + "0.146" + "0" + "0" + "0.146" + "0" + "0.125" + "" + "1.0" + "" + "" + "" + "" + "0.5" + "1.0" + "" + "" + "" + "" + "" + "" + "0.5" + "1.0" + "" + "" + "" + "" + "" + ""); + } + else + { + ignwarn << "Invalid model string " << modelSdfString << "\n"; + ignwarn << "The valid options are:\n"; + ignwarn << " - box\n"; + ignwarn << " - sphere\n"; + ignwarn << " - cylinder\n"; + return; + } + + auto event = new gui::events::SpawnPreviewModel(modelSdfString); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + event); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::Shapes, + ignition::gui::Plugin) diff --git a/src/gui/plugins/shapes/Shapes.hh b/src/gui/plugins/shapes/Shapes.hh new file mode 100644 index 0000000000..3b8e75216b --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.hh @@ -0,0 +1,57 @@ +/* + * 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_GUI_SHAPES_HH_ +#define IGNITION_GAZEBO_GUI_SHAPES_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ + class ShapesPrivate; + + /// \brief Provides buttons for adding a box, sphere, or cylinder + /// to the scene + class Shapes : public ignition::gui::Plugin + { + Q_OBJECT + + /// \brief Constructor + public: Shapes(); + + /// \brief Destructor + public: ~Shapes() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + /// \brief Callback in Qt thread when mode changes. + /// \param[in] _mode New transform mode + public slots: void OnMode(const QString &_mode); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/shapes/Shapes.qml b/src/gui/plugins/shapes/Shapes.qml new file mode 100644 index 0000000000..0db003a072 --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.qml @@ -0,0 +1,88 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Window 2.2 +import QtQuick.Controls 2.1 +import QtQuick.Controls.Material 2.2 +import QtQuick.Controls.Material.impl 2.2 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 + +ToolBar { + id: shapes + Layout.minimumWidth: 200 + Layout.minimumHeight: 100 + + background: Rectangle { + color: "transparent" + } + + RowLayout { + spacing: 2 + ToolButton { + id: box + ToolTip.text: "Box" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "box.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + Shapes.OnMode("box") + } + } + ToolButton{ + id: sphere + ToolTip.text: "Sphere" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "sphere.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + Shapes.OnMode("sphere") + } + } + ToolButton { + id: cylinder + ToolTip.text: "Cylinder" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "cylinder.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + Shapes.OnMode("cylinder") + } + } + } +} diff --git a/src/gui/plugins/shapes/Shapes.qrc b/src/gui/plugins/shapes/Shapes.qrc new file mode 100644 index 0000000000..0aad2596be --- /dev/null +++ b/src/gui/plugins/shapes/Shapes.qrc @@ -0,0 +1,8 @@ + + + Shapes.qml + box.png + sphere.png + cylinder.png + + diff --git a/src/gui/plugins/shapes/box.png b/src/gui/plugins/shapes/box.png new file mode 100644 index 0000000000000000000000000000000000000000..9064dfc0abbad8e2291b671116e3f1e46bef23fe GIT binary patch literal 863 zcmeAS@N?(olHy`uVBq!ia0vp^b|B2b3?yCTBg%jjOS+@4BLl<6e(pbstU$g(vPY0F z14ET614BbI1H;e%K>8&EL#Y7+!>a@a2CEqi4C48d;*Yuk)m{zo332`Z|NnKO!OfdD zfogBxzJ2G;olBQ4odg3Q8!Ldw!byl4I2X_R` z0<_}n*|TTPoCPYrbm{WNix-yn;M(d0RFt1M0E%ba4#vIR5tf$*5)niPnc6GoxfyKL`<>Big)DDWk(8F~$73tYZGbQYODrt8GfB*X_@<6MMA1F^{KuOo?PkiyTb2ny|CL@+JWgEcfZ))fbz6cLcow!Epd$~ zNl7e8wMs5Z1yT$~28L$329~;p#vz90R)$7a#s<0u=2iv3oc@92OHf?)F%lREJuUM3zRMMQvXH||Kqz}DLXtd$zB-}W5ju_` zG%5gX2||JC*LKR*B1EEa*YnvU6q+fr0-1{*I0gxm@1f-Y%2L`uh5mN@ed%^z`&76pHSd znAPI=puvC>gxP2`0fW&bd0dE0#WoY#W6eEpU*11#^QOVi)Gm!sR62Xo+Dz=1mF?o*xh#Jg34YJ`H-( z?)X?1I`107fyUzqC+QeMUjDz6gvxFO!O%?Q{_@p<9#(@y98Gh^0XvGSxWDte+P#k(NzUh5Z^ zRXJ9_mY|Zdd8qJcfbcU$am{UzKG;x<05Tg;$IE%Sx+tX8RFhe{pqaQ)+`FDWWC0mfw6kL5yqed~U32Zi>0u z`{6zN?2Rv~@`7&2i?q+sHY+)Z^73u6|uL9_Usbvr3*9mLP22| zErNUfq;{O8s&x2kdKymCu))S^6_ZMX= B0}cQH literal 0 HcmV?d00001 diff --git a/src/gui/plugins/shapes/sphere.png b/src/gui/plugins/shapes/sphere.png new file mode 100644 index 0000000000000000000000000000000000000000..824befa12efc96cda444668dd29443a342820880 GIT binary patch literal 1449 zcmV;a1y=frP)004R=004l4008;_004mL004C`008P>0026e000+nl3&F}00002 zVoOIv0RM-N%)bBt010qNS#tmY3ljhU3ljkVnw%H_000McNliru;0X;83?6Q@u%-Y2 z1e{4kK~!koty#^E+(Z!my6xF$S7?>Lmmn*F01;`!JV2fSB=*XiEd9wU0q$>?sm4UnXx^#zpuWk?yhbdLcKjAzyKf+y88(h z4L&pV&oVUx9+8FZ_o@H5o(SN2G7EfOL8OqtOL`3pGeCP#4(*Q}0N~Q`wNKB9zzJCx zc&8}%5F--q5rFN2pxFH1r{6uJ1zI$)=xB@^vU>&wZ*hH1_F=w7HYID+dPpnvD43ZS2O@^X01UwAA0_)#wAUy`(4J;dq?1b>;)y3-DK!FuCEWU^`9)qWRqy9TgMieRF zHLzX??v)W4z>`sHgmcO|;K&XHUJ&o&Ayj}MRi1!(V*hOh0v58>>tJ%Ag(`4bbd8<` z*1zh#n&zoqnoWDF+xiw@2+wID;_Gu?QXp*L;IQd`Wn+v}Ft3Nf7sgJOg^_5fk|TXb z?q3^5l!CuWT&{tpGoUOumOp-k(cvBY=tWt*nLlYq8G^%}&v(Oq+a%&&mjM8A+h;od2a>3j#jjOrQg?x@TMo zNEII;?K&#$26jk7A3vk`PXP%IE&We-QnA@C1mlIi^x{F0^Fg^mTw4b1@!DOy4aAQH z6`5E4{VB5fB>4dE%tVpNZ)1|Ro%bvdv!E#S~(n~ntHzR!pD)&dSz5kIAYwgwVP zUn)N%kfuPvH!0zO`dwxX`m+2nHHrC-zBa-KZ%zJE5=(=wD|Sd%yLyz*1AYhrCNdz@ zAqJZp{RY4=GdijYf(~Ene>XGg>k)%Z&qybLMAFTNALbbTmWRQDtfZi4`+WFuc05#M zCP+m&7UU?*fgh&=Y>DOp8RRD2ahEd}o-fjjH7PFOU2>9=Qe)o4Y**C+pBOWY_Z(XG z{9$vfhd20$$t>22?~^ty)kgdIlhS4I_-L{EkaGP&-Lz{OUYVI*{!!NbdJy8tTZvu% zTAops>(@V>eFddC))w3f{K2(51q6Wg&;NXxUWl0k4FFdTKD-m3O}?`EgD8^;^|nlS zlzC@#pEk4MLd(51$}ErUo2$Q^2mF<9axbkW-y6Q44h{AvR`rVBoX#UL4PHzD0G`mB z3h>>ayFC{IogLHb{NwNeKCW8JuOu+qUx%;una98H-JJKF{{cyyM}(}6@!HcdA00000NkvXXu0mjf DTMvDU literal 0 HcmV?d00001 diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 505dc00372..361ab8315a 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -89,6 +89,12 @@ void SceneManager::SetWorldId(Entity _id) this->dataPtr->worldId = _id; } +///////////////////////////////////////////////// +Entity SceneManager::WorldId() const +{ + return this->dataPtr->worldId; +} + ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::CreateModel(Entity _id, const sdf::Model &_model, Entity _parentId) From 6faa218fefffe17ffa3f64434c3053c2ddaaea2e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 15 May 2020 15:22:58 -0500 Subject: [PATCH 03/23] Fix failing test Signed-off-by: Addisu Z. Taddese --- test/integration/triggered_publisher.cc | 1 - test/worlds/triggered_publisher.sdf | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index ac230a8598..9b50f024c0 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -277,7 +277,6 @@ TEST_F(TriggeredPublisherTest, FieldMatchersWithRepeatedFieldsUsePartialMatches) ++recvCount; }); node.Subscribe("/out_7", msgCb); - server->Run(true, 100, false); const int pubCount{10}; for (int i = 0; i < pubCount; ++i) diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 19ce5cb1d6..7353322f29 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -87,7 +87,7 @@ } - + From d67cff9e59ef79a8f9d4c54944f7a2653bdf9d1a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 15 May 2020 20:52:49 -0500 Subject: [PATCH 04/23] More tests and error handling Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 171 +++++++++++++----- .../triggered_publisher/TriggeredPublisher.hh | 30 ++- test/integration/triggered_publisher.cc | 95 ++++++++-- test/worlds/triggered_publisher.sdf | 19 +- 4 files changed, 257 insertions(+), 58 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 9f69673e57..15c08b8f30 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -31,18 +31,35 @@ using namespace systems; /// \brief Base class for input matchers. class systems::InputMatcher { + /// \brief Constructor + /// \param[in] _msgType Input message type + public: InputMatcher(const std::string &_msgType); + + /// \brief Destructor + public: virtual ~InputMatcher() = default; + /// \brief Match input message against the match criteria. /// \param[in] _input Input message /// \return True if the input matches the match criteria. - public: virtual bool Match(const transport::ProtoMsg &_input) const = 0; + public: bool Match(const transport::ProtoMsg &_input) const; - /// \brief Destructor - public: virtual ~InputMatcher() = default; + /// \brief Match input message against the match criteria. Subclasses should + /// override this + /// \param[in] _input Input message + /// \return True if the input matches the match criteria. + public: virtual bool DoMatch(const transport::ProtoMsg &_input) const = 0; /// \brief Checks if the matcher is in a valid state. /// \return True if the matcher is in a valid state. public: virtual bool IsValid() const; + /// \brief Helper function that checks if two messages have the same type + /// \input[in] _matcher Matcher message + /// \input[in] _input Input message + /// \return True if the two message types match + public: static bool CheckTypeMatch(const transport::ProtoMsg &_matcher, + const transport::ProtoMsg &_input); + /// \brief Factory function for creating matchers. /// \param[in] _msgType Input message type (eg. ignition.msgs.Boolean) /// \param[in] _matchElem the SDFormat Element that contains the configuration @@ -52,16 +69,23 @@ class systems::InputMatcher public: static std::unique_ptr Create( const std::string &_msgType, const sdf::ElementPtr &_matchElem); + /// \brief Protobuf message for matching against input + protected: std::unique_ptr matchMsg; + /// \brief State of the matcher protected: bool valid{false}; }; ////////////////////////////////////////////////// -/// \brief Matches any input message +/// \brief Matches any input message of the specified type class AnyMatcher : public InputMatcher { + /// \brief Constructor + /// \param[in] _msgType Input message type + public: AnyMatcher(const std::string &_msgType); + // Documentation inherited - public: bool Match(const transport::ProtoMsg &_input) const override; + public: bool DoMatch(const transport::ProtoMsg &_input) const override; }; @@ -83,10 +107,7 @@ class FullMatcher : public InputMatcher const std::string &_matchString); // Documentation inherited - public: bool Match(const transport::ProtoMsg &_input) const override; - - /// \brief Protobuf message for matching against input - protected: std::unique_ptr matchMsg; + public: bool DoMatch(const transport::ProtoMsg &_input) const override; /// \brief Logic type of this matcher protected: const bool logicType; @@ -112,16 +133,23 @@ class FieldMatcher : public InputMatcher const std::string &_fieldString); // Documentation inherited - public: bool Match(const transport::ProtoMsg &_input) const override; + public: bool DoMatch(const transport::ProtoMsg &_input) const override; + + /// \brief Helper function to find a subfield inside the message based on the + /// given field name. + /// \param[in] _msg The message containing the subfield + /// \param[in] _fieldName Field name inside the message. Each period ('.') + /// character is used to indicate a subfield. + /// \param[out] _fieldDesc Field descriptors found while traversing the + /// message to find the field + /// \param[out] _subMsg Submessage of the field that corresponds to the field + /// name protected: static bool FindFieldSubMessage( transport::ProtoMsg *_msg, const std::string &_fieldName, std::vector &_fieldDesc, transport::ProtoMsg **_subMsg); - /// \brief Protobuf message for matching against input - protected: std::unique_ptr matchMsg; - /// \brief Logic type of this matcher protected: const bool logicType; @@ -131,44 +159,85 @@ class FieldMatcher : public InputMatcher /// \brief Field descriptor of the field compared by this matcher protected: std::vector fieldDescMatcher; + + protected: google::protobuf::util::DefaultFieldComparator comparator; + protected: mutable google::protobuf::util::MessageDifferencer diff; }; ////////////////////////////////////////////////// -bool AnyMatcher::Match(const transport::ProtoMsg &) const +InputMatcher::InputMatcher(const std::string &_msgType) + : matchMsg(msgs::Factory::New(_msgType)) +{ +} + +////////////////////////////////////////////////// +bool InputMatcher::Match( const transport::ProtoMsg &_input) const +{ + if (!CheckTypeMatch(*this->matchMsg, _input)) + { + return false; + } + return this->DoMatch(_input); +} + +////////////////////////////////////////////////// +bool InputMatcher::CheckTypeMatch(const transport::ProtoMsg &_matcher, + const transport::ProtoMsg &_input) +{ + const auto *matcherDesc = _matcher.GetDescriptor(); + const auto *inputDesc = _input.GetDescriptor(); + if (matcherDesc != inputDesc) + { + ignerr << "Received message has a different type than configured in " + << ". Expected [" << matcherDesc->full_name() << "] got [" + << inputDesc->full_name() << "]\n"; + return false; + } + return true; +} + +////////////////////////////////////////////////// +AnyMatcher::AnyMatcher(const std::string &_msgType) : InputMatcher(_msgType) +{ + this->valid = (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()); +} + +////////////////////////////////////////////////// +bool AnyMatcher::DoMatch(const transport::ProtoMsg &) const { return true; } ////////////////////////////////////////////////// FullMatcher::FullMatcher(const std::string &_msgType, bool _logicType, - const std::string &_matchString) - : matchMsg(msgs::Factory::New(_msgType, _matchString)), - logicType(_logicType) + const std::string &_matchString) + : InputMatcher(_msgType), logicType(_logicType) { - if (nullptr != matchMsg) - this->valid = true; + if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + return; + + this->valid = google::protobuf::TextFormat::ParseFromString( + _matchString, this->matchMsg.get()); } ////////////////////////////////////////////////// -bool FullMatcher::Match( - const transport::ProtoMsg &_input) const +bool FullMatcher::DoMatch(const transport::ProtoMsg &_input) const { return this->logicType == google::protobuf::util::MessageDifferencer::ApproximatelyEquals( *this->matchMsg, _input); } + ////////////////////////////////////////////////// FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, const std::string &_fieldName, const std::string &_fieldString) - : matchMsg(msgs::Factory::New(_msgType)), + : InputMatcher(_msgType), logicType(_logicType), fieldName(_fieldName) { - if (nullptr == this->matchMsg) - { + if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) return; - } transport::ProtoMsg *matcherSubMsg{nullptr}; FindFieldSubMessage(this->matchMsg.get(), _fieldName, this->fieldDescMatcher, @@ -197,8 +266,19 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, return; } + this->comparator.set_float_comparison( + google::protobuf::util::DefaultFieldComparator::APPROXIMATE); + + this->diff.set_field_comparator(&comparator); + this->diff.set_scope(google::protobuf::util::MessageDifferencer::PARTIAL); + this->diff.set_repeated_field_comparison( + google::protobuf::util::MessageDifferencer::AS_SET); + this->diff.set_message_field_comparison( + google::protobuf::util::MessageDifferencer::EQUIVALENT); + this->valid = true; } + ////////////////////////////////////////////////// bool FieldMatcher::FindFieldSubMessage( transport::ProtoMsg *_msg, const std::string &_fieldName, @@ -252,12 +332,13 @@ bool FieldMatcher::FindFieldSubMessage( } ////////////////////////////////////////////////// -bool FieldMatcher::Match( +bool FieldMatcher::DoMatch( const transport::ProtoMsg &_input) const { google::protobuf::util::DefaultFieldComparator comp; - auto *reflection = this->matchMsg->GetReflection(); + auto *matcherRefl = this->matchMsg->GetReflection(); + auto *inputRefl = _input.GetReflection(); const transport::ProtoMsg *subMsgMatcher = this->matchMsg.get(); const transport::ProtoMsg *subMsgInput = &_input; for (std::size_t i = 0; i < this->fieldDescMatcher.size() - 1; ++i) @@ -266,25 +347,21 @@ bool FieldMatcher::Match( if (fieldDesc->is_repeated()) { subMsgMatcher = - &reflection->GetRepeatedMessage(*subMsgMatcher, fieldDesc, 0); + &matcherRefl->GetRepeatedMessage(*subMsgMatcher, fieldDesc, 0); subMsgInput = - &reflection->GetRepeatedMessage(*subMsgInput, fieldDesc, 0); + &inputRefl->GetRepeatedMessage(*subMsgInput, fieldDesc, 0); } else { - subMsgMatcher = &reflection->GetMessage(*subMsgMatcher, fieldDesc); - subMsgInput = &reflection->GetMessage(*subMsgInput, fieldDesc); + subMsgMatcher = &matcherRefl->GetMessage(*subMsgMatcher, fieldDesc); + subMsgInput = &inputRefl->GetMessage(*subMsgInput, fieldDesc); } } - google::protobuf::util::MessageDifferencer diff; - diff.set_scope(google::protobuf::util::MessageDifferencer::PARTIAL); - diff.set_repeated_field_comparison( - google::protobuf::util::MessageDifferencer::AS_SET); return this->logicType == - diff.CompareWithFields(*subMsgMatcher, *subMsgInput, - {this->fieldDescMatcher.back()}, - {this->fieldDescMatcher.back()}); + this->diff.CompareWithFields(*subMsgMatcher, *subMsgInput, + {this->fieldDescMatcher.back()}, + {this->fieldDescMatcher.back()}); } ////////////////////////////////////////////////// @@ -299,7 +376,7 @@ std::unique_ptr InputMatcher::Create( { if (nullptr == _matchElem) { - return std::make_unique(); + return std::make_unique(_msgType); } std::unique_ptr matcher{nullptr}; @@ -321,7 +398,8 @@ std::unique_ptr InputMatcher::Create( } if (matcher == nullptr || !matcher->IsValid()) { - ignerr << "Matcher type could not be created from:\n" + ignerr << "Matcher for input type [" << _msgType + << "] could not be created from:\n" << inputMatchString << std::endl; return nullptr; } @@ -329,7 +407,6 @@ std::unique_ptr InputMatcher::Create( return matcher; } - ////////////////////////////////////////////////// TriggeredPublisher::~TriggeredPublisher() { @@ -340,6 +417,7 @@ TriggeredPublisher::~TriggeredPublisher() this->workerThread.join(); } } + ////////////////////////////////////////////////// void TriggeredPublisher::Configure(const Entity &, const std::shared_ptr &_sdf, @@ -415,7 +493,7 @@ void TriggeredPublisher::Configure(const Entity &, } else { - ignerr << "No input specified" << std::endl; + ignerr << "No ouptut specified" << std::endl; return; } @@ -484,7 +562,14 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) return std::all_of(this->matchers.begin(), this->matchers.end(), [&](const auto &matcher) { - return matcher->Match(_inputMsg); + try + { + return matcher->Match(_inputMsg); + } catch (const google::protobuf::FatalException &_err) + { + ignerr << _err.what() << std::endl; + return false; + } }); } diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 357ec65695..4a480e464a 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -39,7 +39,7 @@ namespace systems /// on an output topic in response to an input message that matches user /// specified criteria. /// - /// System Parameters + /// ## System Parameters /// /// `` The tag contains the input message type, topic and matcher /// information. @@ -54,7 +54,7 @@ namespace systems /// comparison must succeed or fail in order to trigger an output /// message. A "positive" value triggers a match when a comparison /// succeeds. A "negative" value triggers a match when a comparson - /// fails. + /// fails. The default value is "positive" /// * `field`: If specified, specified, only this field inside the input /// message is compared for a match. /// * Value: String used to construct the protobuf message against which @@ -74,13 +74,16 @@ namespace systems /// /// Examples: /// 1. Any receipt of a Boolean messages on the input topic triggers an event + /// \code{.xml} /// /// /// /// + /// \endcode /// /// 2. Exact match: An output is triggered when a Boolean message with a value /// of "true" is received + /// \code{.xml} /// /// /// @@ -89,8 +92,10 @@ namespace systems /// /// /// + /// \endcode /// /// 3. Field match: An output is triggered when a specific field matches + /// \code{.xml} /// /// /// 1.0 @@ -98,6 +103,27 @@ namespace systems /// /// /// + /// \endcode + /// + /// The `logic_type` attribute can be used to negate a match. That is, to + /// trigger an output when the input does not equal the value in + /// For example, the following will trigger an ouput when the input does not + /// equal 1 AND does not equal 2. + /// \code{.xml} + /// + /// + /// 1 + /// 2 + /// + /// + /// + /// \endcode + /// + /// ### Repeated Fields + /// Repeated fields can be fully or partially matched. To do a full match + /// with a, the + /// `field` attribute must be set to the containing field of the repeated + /// field. For example, to do a full match of an ignition.msgs.Int32_V, class IGNITION_GAZEBO_VISIBLE TriggeredPublisher : public System, public ISystemConfigure { diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 9b50f024c0..39af3bc069 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -83,13 +83,36 @@ TEST_F(TriggeredPublisherTest, EmptyInputEmptyOutput) const std::size_t pubCount{10}; for (std::size_t i = 0; i < pubCount; ++i) { - inputPub.Publish(msgs::Empty()); + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); server->Run(true, 10, false); } EXPECT_EQ(pubCount, recvCount); } +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, WrongInputMessageTypeDoesNotMatch) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_0"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_0", msgCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Boolean())); + server->Run(true, 10, false); + } + + EXPECT_EQ(0u, recvCount); +} + ///////////////////////////////////////////////// TEST_F(TriggeredPublisherTest, InputMessagesTriggerOutputs) { @@ -106,7 +129,7 @@ TEST_F(TriggeredPublisherTest, InputMessagesTriggerOutputs) const std::size_t pubCount{10}; for (std::size_t i = 0; i < pubCount; ++i) { - inputPub.Publish(msgs::Empty()); + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); server->Run(true, 10, false); } @@ -137,7 +160,7 @@ TEST_F(TriggeredPublisherTest, MultipleOutputsForOneInput) const int pubCount{10}; for (int i = 0; i < pubCount; ++i) { - inputPub.Publish(msgs::Empty()); + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); server->Run(true, 10, false); } @@ -165,11 +188,11 @@ TEST_F(TriggeredPublisherTest, ExactMatchBooleanInputs) { if (i < trueCount) { - inputPub.Publish(msgs::Convert(true)); + EXPECT_TRUE(inputPub.Publish(msgs::Convert(true))); } else { - inputPub.Publish(msgs::Convert(false)); + EXPECT_TRUE(inputPub.Publish(msgs::Convert(false))); } server->Run(true, 100, false); } @@ -195,7 +218,8 @@ TEST_F(TriggeredPublisherTest, MatcherWithNegativeLogicType) const int pubCount{10}; for (int i = 0; i < pubCount; ++i) { - inputPub.Publish(msgs::Convert(static_cast(i - pubCount / 2))); + EXPECT_TRUE(inputPub.Publish( + msgs::Convert(static_cast(i - pubCount / 2)))); server->Run(true, 100, false); } // The matcher filters out 0 so we expect 9 output messages from the 10 inputs @@ -218,7 +242,8 @@ TEST_F(TriggeredPublisherTest, MultipleMatchersAreAnded) const int pubCount{10}; for (int i = 0; i < pubCount; ++i) { - inputPub.Publish(msgs::Convert(static_cast(i - pubCount / 2))); + EXPECT_TRUE(inputPub.Publish( + msgs::Convert(static_cast(i - pubCount / 2)))); server->Run(true, 100, false); } // The matcher filters out negative numbers and the input is [-5,4], so we @@ -253,7 +278,7 @@ TEST_F(TriggeredPublisherTest, FieldMatchers) for (int i = 0; i < pubCount; ++i) { msg.set_y(static_cast(i)); - inputPub.Publish(msg); + EXPECT_TRUE(inputPub.Publish(msg)); server->Run(true, 100, false); } @@ -292,7 +317,7 @@ TEST_F(TriggeredPublisherTest, FieldMatchersWithRepeatedFieldsUsePartialMatches) auto *other = poseMsg.mutable_header()->add_data(); other->set_key("other_key"); other->add_value("other_value"); - inputPub.Publish(poseMsg); + EXPECT_TRUE(inputPub.Publish(poseMsg)); server->Run(true, 100, false); } @@ -336,7 +361,7 @@ TEST_F(TriggeredPublisherTest, auto *other = poseMsg.mutable_header()->add_data(); other->set_key("other_key"); other->add_value("other_value"); - inputPub.Publish(poseMsg); + EXPECT_TRUE(inputPub.Publish(poseMsg)); server->Run(true, 100, false); } @@ -346,6 +371,29 @@ TEST_F(TriggeredPublisherTest, // inputs. EXPECT_EQ(1u, recvCount); } +TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedSubFieldExpected) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_7"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_7", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + msgs::Empty msg; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msg)); + server->Run(true, 100, false); + } + + EXPECT_EQ(0u, recvCount); +} ///////////////////////////////////////////////// /// Tests that full matchers can be used with repeated fields by specifying the @@ -370,7 +418,7 @@ TEST_F(TriggeredPublisherTest, for (int i = 0; i < pubCount; ++i) { msg.add_data(i); - inputPub.Publish(msg); + EXPECT_TRUE(inputPub.Publish(msg)); server->Run(true, 100, false); } @@ -378,3 +426,28 @@ TEST_F(TriggeredPublisherTest, // The matcher only matches {0,1} EXPECT_EQ(1u, recvCount); } + +TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedFieldExpected) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_9"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_9", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + msgs::Int32 msg; + for (int i = 0; i < pubCount; ++i) + { + msg.set_data(i); + EXPECT_TRUE(inputPub.Publish(msg)); + server->Run(true, 100, false); + } + + EXPECT_EQ(0u, recvCount); +} diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 7353322f29..8a097bd53f 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -106,12 +106,27 @@ + + + + + + data: 0, data: 1 + + + + - + data: 0, data: 1 - + + + + + + From 5657f0d2855b13e7c14cf2cae05e3e192c960c60 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 18 May 2020 09:56:44 -0500 Subject: [PATCH 05/23] Add tolerance for float comparisons Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 46 ++++--- test/integration/triggered_publisher.cc | 121 +++++++++++++----- test/worlds/triggered_publisher.sdf | 28 +++- 3 files changed, 140 insertions(+), 55 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 15c08b8f30..2e6e9fd076 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -53,6 +53,8 @@ class systems::InputMatcher /// \return True if the matcher is in a valid state. public: virtual bool IsValid() const; + public: void SetTolerance(double _tol); + /// \brief Helper function that checks if two messages have the same type /// \input[in] _matcher Matcher message /// \input[in] _input Input message @@ -74,6 +76,9 @@ class systems::InputMatcher /// \brief State of the matcher protected: bool valid{false}; + + protected: google::protobuf::util::DefaultFieldComparator comparator; + protected: mutable google::protobuf::util::MessageDifferencer diff; }; ////////////////////////////////////////////////// @@ -159,15 +164,18 @@ class FieldMatcher : public InputMatcher /// \brief Field descriptor of the field compared by this matcher protected: std::vector fieldDescMatcher; - - protected: google::protobuf::util::DefaultFieldComparator comparator; - protected: mutable google::protobuf::util::MessageDifferencer diff; }; ////////////////////////////////////////////////// InputMatcher::InputMatcher(const std::string &_msgType) : matchMsg(msgs::Factory::New(_msgType)) { + this->comparator.set_float_comparison( + google::protobuf::util::DefaultFieldComparator::APPROXIMATE); + + this->diff.set_field_comparator(&this->comparator); + // this->diff.set_message_field_comparison( + // google::protobuf::util::MessageDifferencer::EQUIVALENT); } ////////////////////////////////////////////////// @@ -180,6 +188,12 @@ bool InputMatcher::Match( const transport::ProtoMsg &_input) const return this->DoMatch(_input); } +void InputMatcher::SetTolerance(double _tol) +{ + this->comparator.SetDefaultFractionAndMargin( + std::numeric_limits::min(), _tol); +} + ////////////////////////////////////////////////// bool InputMatcher::CheckTypeMatch(const transport::ProtoMsg &_matcher, const transport::ProtoMsg &_input) @@ -223,9 +237,7 @@ FullMatcher::FullMatcher(const std::string &_msgType, bool _logicType, ////////////////////////////////////////////////// bool FullMatcher::DoMatch(const transport::ProtoMsg &_input) const { - return this->logicType == - google::protobuf::util::MessageDifferencer::ApproximatelyEquals( - *this->matchMsg, _input); + return this->logicType == this->diff.Compare(*this->matchMsg, _input); } ////////////////////////////////////////////////// @@ -244,7 +256,16 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, &matcherSubMsg); if (this->fieldDescMatcher.empty()) + { return; + } + else if (this->fieldDescMatcher.back()->is_repeated()) + { + this->diff.set_scope(google::protobuf::util::MessageDifferencer::PARTIAL); + this->diff.set_repeated_field_comparison( + google::protobuf::util::MessageDifferencer::AS_SET); + } + if (nullptr == matcherSubMsg) return; @@ -266,16 +287,6 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, return; } - this->comparator.set_float_comparison( - google::protobuf::util::DefaultFieldComparator::APPROXIMATE); - - this->diff.set_field_comparator(&comparator); - this->diff.set_scope(google::protobuf::util::MessageDifferencer::PARTIAL); - this->diff.set_repeated_field_comparison( - google::protobuf::util::MessageDifferencer::AS_SET); - this->diff.set_message_field_comparison( - google::protobuf::util::MessageDifferencer::EQUIVALENT); - this->valid = true; } @@ -403,6 +414,9 @@ std::unique_ptr InputMatcher::Create( << inputMatchString << std::endl; return nullptr; } + + const auto tol = _matchElem->Get("tol", 1e-8).first; + matcher->SetTolerance(tol); } return matcher; } diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 39af3bc069..ae4e7e32bb 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -328,16 +328,10 @@ TEST_F(TriggeredPublisherTest, FieldMatchersWithRepeatedFieldsUsePartialMatches) EXPECT_EQ(1u, recvCount); } -///////////////////////////////////////////////// -/// Tests that field matchers can be used to do a full match with repeated -/// fields by specifying the containing field of the repeated field in the -/// "field" attribute and setting the desired values of the repeated field in -/// the value of the tag. -TEST_F(TriggeredPublisherTest, - FieldMatchersWithRepeatedFieldsInValueUseFullMatches) +TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedSubFieldExpected) { transport::Node node; - auto inputPub = node.Advertise("/in_7"); + auto inputPub = node.Advertise("/in_7"); std::size_t recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) @@ -348,51 +342,57 @@ TEST_F(TriggeredPublisherTest, server->Run(true, 100, false); const int pubCount{10}; + msgs::Empty msg; for (int i = 0; i < pubCount; ++i) { - msgs::Pose poseMsg; - auto *frame = poseMsg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value(std::string("frame") + std::to_string(i)); - - auto *time = poseMsg.mutable_header()->mutable_stamp(); - time->set_sec(10); - - auto *other = poseMsg.mutable_header()->add_data(); - other->set_key("other_key"); - other->add_value("other_value"); - EXPECT_TRUE(inputPub.Publish(poseMsg)); + EXPECT_TRUE(inputPub.Publish(msg)); server->Run(true, 100, false); } - // The matcher filters out frame ids that are not frame0, so we expect 1 - // output. Even though the data field contains other key-value pairs, since - // repeated fields use partial matching, the matcher will match one of the - // inputs. - EXPECT_EQ(1u, recvCount); + EXPECT_EQ(0u, recvCount); } -TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedSubFieldExpected) + +///////////////////////////////////////////////// +/// Tests that field matchers can be used to do a full match with repeated +/// fields by specifying the containing field of the repeated field in the +/// "field" attribute and setting the desired values of the repeated field in +/// the value of the tag. +TEST_F(TriggeredPublisherTest, + FieldMatchersWithRepeatedFieldsInValueUseFullMatches) { transport::Node node; - auto inputPub = node.Advertise("/in_7"); + auto inputPub = node.Advertise("/in_8"); std::size_t recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); - node.Subscribe("/out_7", msgCb); + node.Subscribe("/out_8", msgCb); server->Run(true, 100, false); const int pubCount{10}; - msgs::Empty msg; for (int i = 0; i < pubCount; ++i) { - EXPECT_TRUE(inputPub.Publish(msg)); + msgs::Pose poseMsg; + auto *frame = poseMsg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value("frame0"); + + if (i < 5) + { + auto *other = poseMsg.mutable_header()->add_data(); + other->set_key("other_key"); + other->add_value("other_value"); + } + EXPECT_TRUE(inputPub.Publish(poseMsg)); server->Run(true, 100, false); } - EXPECT_EQ(0u, recvCount); + // Since the field specified in "field" is not a repeated field, a full match + // is required to trigger an output. Only the first 5 input messages have the + // second "data" entry, so the expected recvCount is 5. + EXPECT_EQ(5u, recvCount); } ///////////////////////////////////////////////// @@ -427,10 +427,67 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(1u, recvCount); } +TEST_F(TriggeredPublisherTest, FullMatchersAcceptToleranceParam) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_10"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_10", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + msgs::Float msg; + for (int i = 0; i < pubCount; ++i) + { + msg.set_data(static_cast(i)* 0.1); + EXPECT_TRUE(inputPub.Publish(msg)); + server->Run(true, 100, false); + } + + // The input contains the sequence {0, 0.1, 0.2, ...}, the matcher is set to + // match 0.5 with a tolerance of 0.15, so it should match {0.4, 0.5, 0.6} + EXPECT_EQ(3u, recvCount); +} + +TEST_F(TriggeredPublisherTest, FieldMatchersAcceptToleranceParam) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_11"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_11", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + msgs::Pose msg; + for (int i = 0; i < pubCount; ++i) + { + msg.mutable_position()->set_x(0.1); + msg.mutable_position()->set_z(static_cast(i)* 0.1); + EXPECT_TRUE(inputPub.Publish(msg)); + server->Run(true, 100, false); + } + + // The input contains the sequence {0, 0.1, 0.2, ...} in position.z, the + // matcher is set to match 0.5 with a tolerance of 0.15, so it should match + // {0.4, 0.5, 0.6} + EXPECT_EQ(3u, recvCount); +} + + TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedFieldExpected) { transport::Node node; - auto inputPub = node.Advertise("/in_9"); + auto inputPub = node.Advertise("/invalid_topic"); std::size_t recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 8a097bd53f..3e0e69bf2c 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -107,24 +107,38 @@ - - - + data: 0, data: 1 - + + + + + + data: 0.5 + + - + + 0.5 + + + + + + + + data: 0, data: 1 - + - + From 0d836fe31cd8399ab63f23c5e8a171b2b14fb8e7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 18 May 2020 15:48:41 -0500 Subject: [PATCH 06/23] Parse logic_type as string Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 12 ++++++- test/integration/triggered_publisher.cc | 32 +++++++++++++------ test/worlds/triggered_publisher.sdf | 11 ++++++- 3 files changed, 43 insertions(+), 12 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 2e6e9fd076..0585a48440 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -392,7 +392,17 @@ std::unique_ptr InputMatcher::Create( std::unique_ptr matcher{nullptr}; - const auto logicType = _matchElem->Get("logic_type", true).first; + const auto logicTypeStr = + _matchElem->Get("logic_type", "positive").first; + if (logicTypeStr != "positive" && logicTypeStr != "negative") + { + ignerr << "Unrecognized logic_type attribute [" << logicTypeStr + << "] in matcher for input message type [" << _msgType << "]\n"; + return nullptr; + } + + const bool logicType = logicTypeStr == "positive"; + auto inputMatchString = common::trimmed(_matchElem->Get()); if (!inputMatchString.empty()) { diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index ae4e7e32bb..63adb9af55 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -203,17 +203,25 @@ TEST_F(TriggeredPublisherTest, ExactMatchBooleanInputs) } ///////////////////////////////////////////////// -TEST_F(TriggeredPublisherTest, MatcherWithNegativeLogicType) +TEST_F(TriggeredPublisherTest, MatchersWithLogicTypeAttribute) { transport::Node node; auto inputPub = node.Advertise("/in_4"); - std::size_t recvCount{0}; - auto msgCb = std::function( - [&recvCount](const auto &) - { - ++recvCount; - }); - node.Subscribe("/out_4", msgCb); + std::size_t recvCount[2]{0, 0}; + + auto cbCreator = [](std::size_t &_counter) + { + return std::function( + [&_counter](const msgs::Empty &) + { + ++_counter; + }); + }; + + auto msgCb0 = cbCreator(recvCount[0]); + auto msgCb1 = cbCreator(recvCount[1]); + node.Subscribe("/out_4_0", msgCb0); + node.Subscribe("/out_4_1", msgCb1); const int pubCount{10}; for (int i = 0; i < pubCount; ++i) @@ -222,8 +230,12 @@ TEST_F(TriggeredPublisherTest, MatcherWithNegativeLogicType) msgs::Convert(static_cast(i - pubCount / 2)))); server->Run(true, 100, false); } - // The matcher filters out 0 so we expect 9 output messages from the 10 inputs - EXPECT_EQ(9u, recvCount); + // The negative matcher filters out 0 so we expect 9 output messages from the + // 10 inputs + EXPECT_EQ(9u, recvCount[0]); + + // The positive matcher only accepts the input value 0 + EXPECT_EQ(1u, recvCount[1]); } ///////////////////////////////////////////////// diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 3e0e69bf2c..fabad0d357 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -38,7 +38,16 @@ data: 0 - + + + + + + + data: 0 + + + From 8e48b2a051c6ee4688687f215c6a262a0c08abd3 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 18 May 2020 17:40:37 -0500 Subject: [PATCH 07/23] Disallow specifying subfields of repeated fields Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 17 +++++--- .../triggered_publisher/TriggeredPublisher.hh | 43 ++++++++++++++++--- test/integration/triggered_publisher.cc | 29 +++++++++++++ test/worlds/triggered_publisher.sdf | 6 +++ 4 files changed, 81 insertions(+), 14 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 0585a48440..549f804f6f 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -252,8 +252,11 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, return; transport::ProtoMsg *matcherSubMsg{nullptr}; - FindFieldSubMessage(this->matchMsg.get(), _fieldName, this->fieldDescMatcher, - &matcherSubMsg); + if (!FindFieldSubMessage(this->matchMsg.get(), _fieldName, + this->fieldDescMatcher, &matcherSubMsg)) + { + return; + } if (this->fieldDescMatcher.empty()) { @@ -330,7 +333,8 @@ bool FieldMatcher::FindFieldSubMessage( auto *reflection = (*_subMsg)->GetReflection(); if (fieldDesc->is_repeated()) { - *_subMsg = reflection->AddMessage(*_subMsg, fieldDesc); + ignerr << "Matching subfields of repeated messages is not supported\n"; + return false; } else { @@ -357,10 +361,9 @@ bool FieldMatcher::DoMatch( auto *fieldDesc = this->fieldDescMatcher[i]; if (fieldDesc->is_repeated()) { - subMsgMatcher = - &matcherRefl->GetRepeatedMessage(*subMsgMatcher, fieldDesc, 0); - subMsgInput = - &inputRefl->GetRepeatedMessage(*subMsgInput, fieldDesc, 0); + // This should not happen since the matching subfields of repeated fields + // is not allowed and this matcher shouldn't have been created. + ignerr << "Matching subfields of repeated messages is not supported\n"; } else { diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 4a480e464a..a1d2a4f15d 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -44,7 +44,7 @@ namespace systems /// `` The tag contains the input message type, topic and matcher /// information. /// * Attributes: - /// * `type`: Input message type (eg. ignition.msgs.Boolean) + /// * `type`: Input message type (eg. `ignition.msgs.Boolean`) /// * `topic`: Input message topic name /// /// ``: Contains configuration for matchers. Multiple @@ -55,6 +55,7 @@ namespace systems /// message. A "positive" value triggers a match when a comparison /// succeeds. A "negative" value triggers a match when a comparson /// fails. The default value is "positive" + /// * `tol`: Tolerance for floating point comparisons. /// * `field`: If specified, specified, only this field inside the input /// message is compared for a match. /// * Value: String used to construct the protobuf message against which @@ -66,7 +67,7 @@ namespace systems /// tags are possible. A message will be published on each output topic for /// each input that matches. /// * Attributes: - /// * `type`: Output message type (eg. ignition.msgs.Boolean) + /// * `type`: Output message type (eg. `ignition.msgs.Boolean`) /// * `topic`: Output message topic name /// * Value: String used to construct the output protobuf message . This is /// the human-readable representation of a protobuf message as used by @@ -81,7 +82,7 @@ namespace systems /// /// \endcode /// - /// 2. Exact match: An output is triggered when a Boolean message with a value + /// 2. Full match: An output is triggered when a Boolean message with a value /// of "true" is received /// \code{.xml} /// @@ -120,10 +121,38 @@ namespace systems /// \endcode /// /// ### Repeated Fields - /// Repeated fields can be fully or partially matched. To do a full match - /// with a, the - /// `field` attribute must be set to the containing field of the repeated - /// field. For example, to do a full match of an ignition.msgs.Int32_V, + /// When a field matcher is used with repeated fields, the content of the + /// repeated field is treated as a set and the comparison operator is set + /// containment. For example, the `data` field of `ignition.msgs.Int32_V` is a + /// repeated Int32 message. To match an input that contains the values 1 and 2 + /// the following matcher can be used: + /// \code{.xml} + /// + /// + /// 1 + /// 2 + /// + /// + /// + /// \endcode + /// To match an Int32_V message with the exact contents {1, 2}, the full + /// matcher is used instead + /// \code{.xml} + /// + /// + /// + /// data: 1 + /// data: 2 + /// + /// + /// + /// + /// \endcode + /// + /// ### Limitations + /// The current implementation of this system does not support specifying a + /// subfield of a repeated field in the "field" attribute. i.e, if + /// `field="f1.f2"`, `f1` cannot be a repeated field. class IGNITION_GAZEBO_VISIBLE TriggeredPublisher : public System, public ISystemConfigure { diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 63adb9af55..72eb9b3c24 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -495,6 +495,35 @@ TEST_F(TriggeredPublisherTest, FieldMatchersAcceptToleranceParam) EXPECT_EQ(3u, recvCount); } +TEST_F(TriggeredPublisherTest, SubfieldsOfRepeatedFieldsNotSupported) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_12"); + std::size_t recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_12", msgCb); + server->Run(true, 100, false); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + msgs::Header msg; + auto *data = msg.add_data(); + data->set_key("key1"); + data->add_value("value1"); + + EXPECT_TRUE(inputPub.Publish(msg)); + server->Run(true, 100, false); + } + + // Subfields of repeated fiealds are not supported, so no output should be + // triggered. + EXPECT_EQ(0u, recvCount); +} TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedFieldExpected) { diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index fabad0d357..624d4c94b5 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -137,6 +137,12 @@ + + + "value1" + + + From 417eb497c26b3e478e581a373c01b3a5d23001a7 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 19 May 2020 10:06:34 -0500 Subject: [PATCH 08/23] Codecheck, clang-tidy Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 26 ++++++++++++------- .../triggered_publisher/TriggeredPublisher.hh | 2 +- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 549f804f6f..4a037c645f 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -53,6 +53,8 @@ class systems::InputMatcher /// \return True if the matcher is in a valid state. public: virtual bool IsValid() const; + /// \brief Set the float comparison tolerance + /// \param[in] _tol Tolerance for float comparisons public: void SetTolerance(double _tol); /// \brief Helper function that checks if two messages have the same type @@ -77,7 +79,13 @@ class systems::InputMatcher /// \brief State of the matcher protected: bool valid{false}; + /// \brief Field comparator used by MessageDifferencer. This is where + /// tolerance for float comparisons is set protected: google::protobuf::util::DefaultFieldComparator comparator; + + /// \brief MessageDifferencer used for comparing input to matcher. This is + /// mutable because MessageDifferencer::CompareWithFields is not a const + /// function protected: mutable google::protobuf::util::MessageDifferencer diff; }; @@ -87,7 +95,7 @@ class AnyMatcher : public InputMatcher { /// \brief Constructor /// \param[in] _msgType Input message type - public: AnyMatcher(const std::string &_msgType); + public: explicit AnyMatcher(const std::string &_msgType); // Documentation inherited public: bool DoMatch(const transport::ProtoMsg &_input) const override; @@ -174,12 +182,10 @@ InputMatcher::InputMatcher(const std::string &_msgType) google::protobuf::util::DefaultFieldComparator::APPROXIMATE); this->diff.set_field_comparator(&this->comparator); - // this->diff.set_message_field_comparison( - // google::protobuf::util::MessageDifferencer::EQUIVALENT); } ////////////////////////////////////////////////// -bool InputMatcher::Match( const transport::ProtoMsg &_input) const +bool InputMatcher::Match(const transport::ProtoMsg &_input) const { if (!CheckTypeMatch(*this->matchMsg, _input)) { @@ -284,9 +290,9 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, return; } } - catch (const std::exception &_err) + catch (const std::exception &err) { - ignerr << "Creating Field matcher failed: " << _err.what() << std::endl; + ignerr << "Creating Field matcher failed: " << err.what() << std::endl; return; } @@ -587,14 +593,14 @@ void TriggeredPublisher::DoWork() bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) { return std::all_of(this->matchers.begin(), this->matchers.end(), - [&](const auto &matcher) + [&](const auto &_matcher) { try { - return matcher->Match(_inputMsg); - } catch (const google::protobuf::FatalException &_err) + return _matcher->Match(_inputMsg); + } catch (const google::protobuf::FatalException &err) { - ignerr << _err.what() << std::endl; + ignerr << err.what() << std::endl; return false; } }); diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index a1d2a4f15d..bd78adf7a0 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -160,7 +160,7 @@ namespace systems public: TriggeredPublisher() = default; /// \brief Destructor - public: ~TriggeredPublisher(); + public: ~TriggeredPublisher() override; // Documentation inherited public: void Configure(const Entity &_entity, From 621318e40065793f11345af1f1fe44dbdaea19af Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 19 May 2020 11:11:49 -0500 Subject: [PATCH 09/23] More error handling Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 79 +++++++++++-------- 1 file changed, 48 insertions(+), 31 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 4a037c645f..e7c16c4032 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -278,21 +278,13 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, if (nullptr == matcherSubMsg) return; - try + bool result = google::protobuf::TextFormat::ParseFieldValueFromString( + _fieldString, this->fieldDescMatcher.back(), matcherSubMsg); + if (!result) { - bool result = google::protobuf::TextFormat::ParseFieldValueFromString( - _fieldString, this->fieldDescMatcher.back(), matcherSubMsg); - if (!result) - { - ignerr << "Failed to parse matcher string [" - << _fieldString << "] for field [" << this->fieldName - << "] of input message type [" << _msgType << "]\n"; - return; - } - } - catch (const std::exception &err) - { - ignerr << "Creating Field matcher failed: " << err.what() << std::endl; + ignerr << "Failed to parse matcher string [" << _fieldString + << "] for field [" << this->fieldName << "] of input message type [" + << _msgType << "]\n"; return; } @@ -310,42 +302,65 @@ bool FieldMatcher::FindFieldSubMessage( // If fieldMsgType is nullptr, then this is not a composite message and we // shouldn't be using a FieldMatcher if (nullptr == fieldMsgType) + { + ignerr << "FieldMatcher with field name [" << _fieldName + << "] cannot be used because the input message type [" + << fieldMsgType->full_name() << "] does not have any fields\n"; return false; + } *_subMsg = _msg; auto fieldNames = common::split(_fieldName, "."); + if (fieldNames.empty()) + { + ignerr << "Empty field attribute for input message type [" + << fieldMsgType->full_name() << "]\n"; + return false; + } + for (std::size_t i = 0; i < fieldNames.size(); ++i) { auto fieldDesc = fieldMsgType->FindFieldByName(fieldNames[i]); if (nullptr == fieldDesc) - break; - - _fieldDesc.push_back(fieldDesc); - - if (google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE != - fieldDesc->cpp_type()) { - break; + ignerr << "Field name [" << fieldNames[i] + << "] could not be found in message type [" + << fieldMsgType->full_name() << "].\n"; + return false; } - fieldMsgType = fieldDesc->message_type(); - if (nullptr == fieldMsgType) - break; + _fieldDesc.push_back(fieldDesc); if (i < fieldNames.size() - 1) { + if (google::protobuf::FieldDescriptor::CPPTYPE_MESSAGE != + fieldDesc->cpp_type()) + { + ignerr << "Subfield [" << fieldNames[i+1] + << "] could not be found in Submessage type [" + << fieldDesc->full_name() << "].\n"; + return false; + } + auto *reflection = (*_subMsg)->GetReflection(); if (fieldDesc->is_repeated()) { - ignerr << "Matching subfields of repeated messages is not supported\n"; + ignerr + << "Field matcher for field name [" << _fieldName + << "] could not be created because the field [" << fieldDesc->name() + << "] is a repeated message type. Matching subfields of repeated " + << "messages is not supported.\n"; return false; } else { *_subMsg = reflection->MutableMessage(*_subMsg, fieldDesc); } + + // Update fieldMsgType for next iteration + fieldMsgType = fieldDesc->message_type(); } } @@ -507,20 +522,22 @@ void TriggeredPublisher::Configure(const Entity &, info.topic = outputElem->Get("topic"); if (info.topic.empty()) { - ignerr << "Topic cannot be empty\n"; + ignerr << "Output topic cannot be empty\n"; continue; } - info.msgData = - msgs::Factory::New(info.msgType, outputElem->Get()); + const std::string msgStr = outputElem->Get(); + info.msgData = msgs::Factory::New(info.msgType, msgStr); if (nullptr != info.msgData) { - info.pub = this->node.Advertise(info.topic, info.msgType); + info.pub = + this->node.Advertise(info.topic, info.msgData->GetTypeName()); this->outputInfo.push_back(std::move(info)); } else { - ignerr << "Unable to create message of type[" << info.msgType - << "] with data[" << info.msgData << "].\n"; + ignerr << "Unable to create message of type [" << info.msgType + << "] with data [" << msgStr << "] when creating output" + << " publisher on topic " << info.topic << ".\n"; } } } From 2a63d10186f09f9bd6ab0aa9ee4629813d64ee41 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 19 May 2020 11:25:39 -0500 Subject: [PATCH 10/23] Fix typos, add more error handling Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 39 +++++++++++++++++-- .../triggered_publisher/TriggeredPublisher.hh | 6 +-- 2 files changed, 38 insertions(+), 7 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index e7c16c4032..a8598edbb1 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -114,7 +114,7 @@ class FullMatcher : public InputMatcher /// return false. /// \param[in] _matchString String used to construct the protobuf message /// against which input messages are matched. This is the human-readable - /// representation of a protobuf message as used by `ign topic` for pulishing + /// representation of a protobuf message as used by `ign topic` for publishing /// messages public: FullMatcher(const std::string &_msgType, bool _logicType, const std::string &_matchString); @@ -140,7 +140,7 @@ class FieldMatcher : public InputMatcher /// \param[in] _fieldString String used to construct the protobuf message /// against which the specified field in the input messages are matched. This /// is the human-readable representation of a protobuf message as used by `ign - /// topic` for pulishing messages + /// topic` for publishing messages public: FieldMatcher(const std::string &_msgType, bool _logicType, const std::string &_fieldName, const std::string &_fieldString); @@ -477,7 +477,18 @@ void TriggeredPublisher::Configure(const Entity &, { auto inputElem = sdfClone->GetElement("input"); this->inputMsgType = inputElem->Get("type"); + if (this->inputMsgType.empty()) + { + ignerr << "Input message type cannot be empty\n"; + return; + } + this->inputTopic = inputElem->Get("topic"); + if (this->inputTopic.empty()) + { + ignerr << "Input topic cannot be empty\n"; + return; + } if (inputElem->HasElement("match")) { @@ -519,6 +530,11 @@ void TriggeredPublisher::Configure(const Entity &, { OutputInfo info; info.msgType = outputElem->Get("type"); + if (info.msgType.empty()) + { + ignerr << "Output message type cannot be empty\n"; + continue; + } info.topic = outputElem->Get("topic"); if (info.topic.empty()) { @@ -531,7 +547,16 @@ void TriggeredPublisher::Configure(const Entity &, { info.pub = this->node.Advertise(info.topic, info.msgData->GetTypeName()); - this->outputInfo.push_back(std::move(info)); + if (info.pub.Valid()) + { + this->outputInfo.push_back(std::move(info)); + } + else + { + ignerr << "Output publisher could not be created for topic [" + << info.topic << "] with message type [" << info.msgType + << "]\n"; + } } else { @@ -559,7 +584,13 @@ void TriggeredPublisher::Configure(const Entity &, this->newMatchSignal.notify_one(); } }); - this->node.Subscribe(this->inputTopic, msgCb); + if (!this->node.Subscribe(this->inputTopic, msgCb)) + { + ignerr << "Input subscriber could not be created for topic [" + << this->inputTopic << "] with message type [" << this->inputMsgType + << "]\n"; + return; + } std::stringstream ss; ss << "TriggeredPublisher subscribed on " << this->inputTopic diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index bd78adf7a0..759683bf37 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -61,7 +61,7 @@ namespace systems /// * Value: String used to construct the protobuf message against which /// input messages are matched. This is the human-readable /// representation of a protobuf message as used by `ign topic` for - /// pulishing messages + /// publishing messages /// /// ``: Contains configuration for output messages: Multiple /// tags are possible. A message will be published on each output topic for @@ -71,10 +71,10 @@ namespace systems /// * `topic`: Output message topic name /// * Value: String used to construct the output protobuf message . This is /// the human-readable representation of a protobuf message as used by - /// `ign topic` for pulishing messages + /// `ign topic` for publishing messages /// /// Examples: - /// 1. Any receipt of a Boolean messages on the input topic triggers an event + /// 1. Any receipt of a Boolean messages on the input topic triggers an output /// \code{.xml} /// /// From 1b96a92d78dfde607a339fefb1bdc09b6bb111ed Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 19 May 2020 14:07:43 -0500 Subject: [PATCH 11/23] Add waits for received messages so tests don't become flaky Signed-off-by: Addisu Z. Taddese --- test/integration/triggered_publisher.cc | 121 +++++++++++++++--------- 1 file changed, 78 insertions(+), 43 deletions(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 72eb9b3c24..5c6fc9b717 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -64,6 +64,26 @@ class TriggeredPublisherTest : public ::testing::Test public: std::unique_ptr server; }; +/// \brief Helper function to wait until a predicate is true or a timeout occurs +/// \tparam Pred Predicate function of type bool() +/// \param[in] _timeoutMs Timeout in milliseconds +template +bool waitUntil(int _timeoutMs, Pred _pred) +{ + using namespace std::chrono; + auto tStart = steady_clock::now(); + auto sleepDur = milliseconds(std::min(100, _timeoutMs)); + auto waitDuration = milliseconds(_timeoutMs); + while (duration_cast(steady_clock::now() - tStart) < waitDuration) + { + if (_pred()) + { + return true; + } + std::this_thread::sleep_for(sleepDur); + } + return false; +} ///////////////////////////////////////////////// /// Check that empty message types do not need any data to be specified in the @@ -72,20 +92,21 @@ TEST_F(TriggeredPublisherTest, EmptyInputEmptyOutput) { transport::Node node; auto inputPub = node.Advertise("/in_0"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_0", msgCb); + IGN_SLEEP_MS(100ms); const std::size_t pubCount{10}; for (std::size_t i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - server->Run(true, 10, false); } + waitUntil(5000, [&]{return pubCount == recvCount;}); EXPECT_EQ(pubCount, recvCount); } @@ -95,7 +116,7 @@ TEST_F(TriggeredPublisherTest, WrongInputMessageTypeDoesNotMatch) { transport::Node node; auto inputPub = node.Advertise("/in_0"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { @@ -107,9 +128,9 @@ TEST_F(TriggeredPublisherTest, WrongInputMessageTypeDoesNotMatch) for (std::size_t i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Boolean())); - server->Run(true, 10, false); } + waitUntil(5000, [&]{return 0u == recvCount;}); EXPECT_EQ(0u, recvCount); } @@ -118,10 +139,11 @@ TEST_F(TriggeredPublisherTest, InputMessagesTriggerOutputs) { transport::Node node; auto inputPub = node.Advertise("/in_1"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( - [&recvCount](const auto &) + [&recvCount](const auto &_msg) { + EXPECT_TRUE(_msg.data()); ++recvCount; }); node.Subscribe("/out_1", msgCb); @@ -130,9 +152,10 @@ TEST_F(TriggeredPublisherTest, InputMessagesTriggerOutputs) for (std::size_t i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - server->Run(true, 10, false); + IGN_SLEEP_MS(10); } + waitUntil(5000, [&]{return pubCount == recvCount;}); EXPECT_EQ(pubCount, recvCount); } @@ -141,13 +164,15 @@ TEST_F(TriggeredPublisherTest, MultipleOutputsForOneInput) { transport::Node node; auto inputPub = node.Advertise("/in_2"); + std::mutex recvMsgMutex; std::vector recvMsgs0; std::vector recvMsgs1; - auto cbCreator = [](std::vector &_msgVector) + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) { return std::function( - [&_msgVector](const msgs::Boolean &_msg) + [&_msgVector, &recvMsgMutex](const msgs::Boolean &_msg) { + std::lock_guard lock(recvMsgMutex); _msgVector.push_back(_msg.data()); }); }; @@ -161,9 +186,19 @@ TEST_F(TriggeredPublisherTest, MultipleOutputsForOneInput) for (int i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msgs::Empty())); - server->Run(true, 10, false); + IGN_SLEEP_MS(10); } + waitUntil(5000, [&] + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); + + EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); + EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); + // The plugin has two outputs. We expect 10 messages in each output topic EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), false)); EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); @@ -174,7 +209,7 @@ TEST_F(TriggeredPublisherTest, ExactMatchBooleanInputs) { transport::Node node; auto inputPub = node.Advertise("/in_3"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { @@ -194,7 +229,7 @@ TEST_F(TriggeredPublisherTest, ExactMatchBooleanInputs) { EXPECT_TRUE(inputPub.Publish(msgs::Convert(false))); } - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The matcher filters out false messages and the inputs consist of 5 true and @@ -207,9 +242,9 @@ TEST_F(TriggeredPublisherTest, MatchersWithLogicTypeAttribute) { transport::Node node; auto inputPub = node.Advertise("/in_4"); - std::size_t recvCount[2]{0, 0}; + std::atomic recvCount[2]{0, 0}; - auto cbCreator = [](std::size_t &_counter) + auto cbCreator = [](std::atomic &_counter) { return std::function( [&_counter](const msgs::Empty &) @@ -228,7 +263,7 @@ TEST_F(TriggeredPublisherTest, MatchersWithLogicTypeAttribute) { EXPECT_TRUE(inputPub.Publish( msgs::Convert(static_cast(i - pubCount / 2)))); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The negative matcher filters out 0 so we expect 9 output messages from the // 10 inputs @@ -243,7 +278,7 @@ TEST_F(TriggeredPublisherTest, MultipleMatchersAreAnded) { transport::Node node; auto inputPub = node.Advertise("/in_5"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { @@ -256,7 +291,7 @@ TEST_F(TriggeredPublisherTest, MultipleMatchersAreAnded) { EXPECT_TRUE(inputPub.Publish( msgs::Convert(static_cast(i - pubCount / 2)))); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The matcher filters out negative numbers and the input is [-5,4], so we // expect 5 output messages. @@ -268,9 +303,9 @@ TEST_F(TriggeredPublisherTest, FieldMatchers) { transport::Node node; auto inputPub = node.Advertise("/in_6"); - std::size_t recvCount[2]{0, 0}; + std::atomic recvCount[2]{0, 0}; - auto cbCreator = [](std::size_t &_counter) + auto cbCreator = [](std::atomic &_counter) { return std::function( [&_counter](const msgs::Empty &) @@ -291,7 +326,7 @@ TEST_F(TriggeredPublisherTest, FieldMatchers) { msg.set_y(static_cast(i)); EXPECT_TRUE(inputPub.Publish(msg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The first plugin matches x==1 and y==2 which only once out of the 10 inputs @@ -307,7 +342,7 @@ TEST_F(TriggeredPublisherTest, FieldMatchersWithRepeatedFieldsUsePartialMatches) { transport::Node node; auto inputPub = node.Advertise("/in_7"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { @@ -330,7 +365,7 @@ TEST_F(TriggeredPublisherTest, FieldMatchersWithRepeatedFieldsUsePartialMatches) other->set_key("other_key"); other->add_value("other_value"); EXPECT_TRUE(inputPub.Publish(poseMsg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The matcher filters out frame ids that are not frame0, so we expect 1 @@ -344,21 +379,21 @@ TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedSubFieldExpected) { transport::Node node; auto inputPub = node.Advertise("/in_7"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_7", msgCb); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); const int pubCount{10}; msgs::Empty msg; for (int i = 0; i < pubCount; ++i) { EXPECT_TRUE(inputPub.Publish(msg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } EXPECT_EQ(0u, recvCount); @@ -374,14 +409,14 @@ TEST_F(TriggeredPublisherTest, { transport::Node node; auto inputPub = node.Advertise("/in_8"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_8", msgCb); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); const int pubCount{10}; for (int i = 0; i < pubCount; ++i) @@ -398,7 +433,7 @@ TEST_F(TriggeredPublisherTest, other->add_value("other_value"); } EXPECT_TRUE(inputPub.Publish(poseMsg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // Since the field specified in "field" is not a repeated field, a full match @@ -416,14 +451,14 @@ TEST_F(TriggeredPublisherTest, { transport::Node node; auto inputPub = node.Advertise("/in_9"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_9", msgCb); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); const int pubCount{10}; msgs::Int32_V msg; @@ -431,7 +466,7 @@ TEST_F(TriggeredPublisherTest, { msg.add_data(i); EXPECT_TRUE(inputPub.Publish(msg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The input contains an increasing sets of sequences, {0}, {0,1}, {0,1,2}... @@ -443,14 +478,14 @@ TEST_F(TriggeredPublisherTest, FullMatchersAcceptToleranceParam) { transport::Node node; auto inputPub = node.Advertise("/in_10"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_10", msgCb); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); const int pubCount{10}; msgs::Float msg; @@ -458,7 +493,7 @@ TEST_F(TriggeredPublisherTest, FullMatchersAcceptToleranceParam) { msg.set_data(static_cast(i)* 0.1); EXPECT_TRUE(inputPub.Publish(msg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The input contains the sequence {0, 0.1, 0.2, ...}, the matcher is set to @@ -470,14 +505,14 @@ TEST_F(TriggeredPublisherTest, FieldMatchersAcceptToleranceParam) { transport::Node node; auto inputPub = node.Advertise("/in_11"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_11", msgCb); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); const int pubCount{10}; msgs::Pose msg; @@ -486,7 +521,7 @@ TEST_F(TriggeredPublisherTest, FieldMatchersAcceptToleranceParam) msg.mutable_position()->set_x(0.1); msg.mutable_position()->set_z(static_cast(i)* 0.1); EXPECT_TRUE(inputPub.Publish(msg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // The input contains the sequence {0, 0.1, 0.2, ...} in position.z, the @@ -499,14 +534,14 @@ TEST_F(TriggeredPublisherTest, SubfieldsOfRepeatedFieldsNotSupported) { transport::Node node; auto inputPub = node.Advertise("/in_12"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_12", msgCb); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); const int pubCount{10}; for (int i = 0; i < pubCount; ++i) @@ -517,7 +552,7 @@ TEST_F(TriggeredPublisherTest, SubfieldsOfRepeatedFieldsNotSupported) data->add_value("value1"); EXPECT_TRUE(inputPub.Publish(msg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } // Subfields of repeated fiealds are not supported, so no output should be @@ -529,14 +564,14 @@ TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedFieldExpected) { transport::Node node; auto inputPub = node.Advertise("/invalid_topic"); - std::size_t recvCount{0}; + std::atomic recvCount{0}; auto msgCb = std::function( [&recvCount](const auto &) { ++recvCount; }); node.Subscribe("/out_9", msgCb); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); const int pubCount{10}; msgs::Int32 msg; @@ -544,7 +579,7 @@ TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedFieldExpected) { msg.set_data(i); EXPECT_TRUE(inputPub.Publish(msg)); - server->Run(true, 100, false); + IGN_SLEEP_MS(10); } EXPECT_EQ(0u, recvCount); From 1a905ff71f69de19df82a0e69e44960f8c5035c6 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 19 May 2020 18:21:16 -0500 Subject: [PATCH 12/23] Codecheck Signed-off-by: Addisu Z. Taddese --- test/integration/triggered_publisher.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 5c6fc9b717..70230ba09e 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -74,7 +74,8 @@ bool waitUntil(int _timeoutMs, Pred _pred) auto tStart = steady_clock::now(); auto sleepDur = milliseconds(std::min(100, _timeoutMs)); auto waitDuration = milliseconds(_timeoutMs); - while (duration_cast(steady_clock::now() - tStart) < waitDuration) + while (duration_cast(steady_clock::now() - tStart) < + waitDuration) { if (_pred()) { From 2ff244954057f209b2061b6a4a43aeb61128ddde Mon Sep 17 00:00:00 2001 From: Addisu Taddese Date: Tue, 19 May 2020 18:50:14 -0500 Subject: [PATCH 13/23] Add option to suppress warning about missing child model (#132) Signed-off-by: Addisu Z. Taddese --- Changelog.md | 3 +++ src/systems/detachable_joint/DetachableJoint.cc | 6 +++++- src/systems/detachable_joint/DetachableJoint.hh | 7 +++++++ 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/Changelog.md b/Changelog.md index 2012feab6f..4cd4a50ee5 100644 --- a/Changelog.md +++ b/Changelog.md @@ -7,6 +7,9 @@ 1. Allow battery plugin to work with joint force systems. * [Pull Request 120](https://github.com/ignitionrobotics/ign-gazebo/pull/120) +1. DetachableJoint system: Add option to suppress warning about missing child model + * [Pull Request 132](https://github.com/ignitionrobotics/ign-gazebo/pull/132) + 1. Make breadcrumb static after specified time * [Pull Request 90](https://github.com/ignitionrobotics/ign-gazebo/pull/90) diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 7befaf618d..1ed2125a1f 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -97,6 +97,10 @@ void DetachableJoint::Configure(const Entity &_entity, "/detachable_joint/detach"}; this->topic = _sdf->Get("topic", defaultTopic).first; + this->suppressChildWarning = + _sdf->Get("suppress_child_warning", this->suppressChildWarning) + .first; + this->validConfig = true; } @@ -151,7 +155,7 @@ void DetachableJoint::PreUpdate( << " could not be found.\n"; } } - else + else if (!this->suppressChildWarning) { ignwarn << "Child Model " << this->childModelName << " could not be found.\n"; diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index 3fdca0b28a..8c98dfd9aa 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -49,6 +49,10 @@ namespace systems /// creating a fixed joint with a link in the parent model. /// /// (optional): Topic name to be used for detaching connections + /// + /// (optional): If true, the system + /// will not print a warning message if a child model does not exist yet. + /// Otherwise, a warning message is printed. Defaults to false. class IGNITION_GAZEBO_VISIBLE DetachableJoint : public System, @@ -84,6 +88,9 @@ namespace systems /// \brief Topic to be used for detaching connections private: std::string topic; + /// \brief Whether to suppress warning about missing child model. + private: bool suppressChildWarning{false}; + /// \brief Entity of attachment link in the parent model private: Entity parentLinkEntity{kNullEntity}; From eb930725f07854d54fa7cb223115cc2446d9065b Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 19 May 2020 19:42:56 -0500 Subject: [PATCH 14/23] Add tutorial for TriggeredPublisher Signed-off-by: Addisu Z. Taddese --- examples/worlds/triggered_publisher.sdf | 132 ++++++++++++-- tutorials.md.in | 1 + tutorials/triggered_publisher.md | 222 ++++++++++++++++++++++++ 3 files changed, 343 insertions(+), 12 deletions(-) create mode 100644 tutorials/triggered_publisher.md diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 569e9ef217..01f7fa79a9 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -1,4 +1,10 @@ + @@ -22,6 +28,10 @@ filename="libignition-gazebo-scene-broadcaster-system.so" name="ignition::gazebo::systems::SceneBroadcaster"> + + @@ -37,7 +47,7 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - -6 0 6 0 0.5 0 + 3 -9 9 0 0.6 1.3 @@ -103,7 +113,7 @@ 0.01 0.001 - -0.5 0.1 -0.9 + 0.05 -0.1 -0.9 @@ -133,7 +143,7 @@ - 0 2 0.325 0 -0 0 + 0 0 0.325 0 -0 0 -0.151427 -0 0.175 0 -0 0 @@ -409,17 +419,115 @@ 0.3 cmd_vel - - - - data: "true" - - - - linear: {x: 1.5} - + + + + 3 0 0 0 0 0 + true + + + + 0.1 10 0.01 + + + + + 0.1 10 0.01 + + + + + c1 + + + + + vehicle_blue + trigger + + true + + + body + box1 + box_body + /box1/detach + + + body + box2 + box_body + /box2/detach + + + 3 0 8 0 0 0 + + + + 1 1 1 + + + 0.8 0.2 0.2 1 + 1.0 0 0 1 + + + + + 1 1 1 + + + + + /altimeter + 1 + 30 + true + + + + + + 8 0 5 0 0 0 + + + + 1 1 1 + + + 0.2 0.8 0.2 1 + 1.0 0 0 1 + + + + + 1 1 1 + + + + + + + + + linear: {x: 3} + + + + + data: true + + + + + + -7.5 + + + diff --git a/tutorials.md.in b/tutorials.md.in index 34586f8a08..8be419a777 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -17,6 +17,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage battery "Battery": Keep track of battery charge on robot models * \subpage debugging "Debugging": Information about debugging Gazebo. * \subpage detachablejoints "Detachable Joints": Creating models that start off rigidly attached and then get detached during simulation +* \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate events in simulation ## License diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md new file mode 100644 index 0000000000..1d0b83430f --- /dev/null +++ b/tutorials/triggered_publisher.md @@ -0,0 +1,222 @@ +\page triggeredpublisher Triggered Publisher + +The TriggeredPublisher system publishes a user specified message on an output +topic in response to an input message that matches user specified criteria. The +system works by checking the input against a set of Matchers. Matchers +contain string representations of protobuf messages which are compared for +equality or containment with the input message. Matchers can match the whole +input message or only a specific field inside the message. + +This tutorial describes how the Triggered Publisher system can be used to +cause a box to fall from its initial position by detaching a detachable joint +in response to the motion of a vehicle. The tutorial also covers how Triggered +Publisher systems can be chained together by showing how the falling of the box +can trigger another box to fall. The finished world SDFormat file for this +tutorial can be found in `examples/worlds/triggered_publisher.sdf` + +We will use the differential drive vehicle from +`/examples/worlds/diff_drive.sdf`, but modify the input topic of the +`DiffDrive` system to `cmd_vel`. A snippet of the change to the `DiffDrive` +system is shown below: +\code{.xml} + + ... + + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 1.25 + 0.3 + cmd_vel + + +\endcode + +The first TriggeredPublisher we create will demonstrate how we can send +a predetermined `Twist` message to the `DiffDrive` vehicle in response to +a "start" message from the user: + +\code{.xml} + + + + linear: {x: 3} + + +\endcode + +The `` tag sets up the TriggeredPublisher to subscribe to the topic +`/start` with a message type of `ignition.msgs.Empty`. The `` tag +specifies the topic of the output and the actual data to be published. The data +is expressed in the human-readable form of Google Protobuf messages. This is +the same format used by `ign topic` for publishing messages. + +Since the TriggeredPublisher only deals with ignition topics, it can be +anywhere a `` tag is allowed. For this example, we will put it under +``. + +Next we will create a trigger that causes a box to fall when the `DiffDrive` vehicle crosses a contact sensor on the ground. To do this, +we first create the falling box model and call it `box1` + +\code{.xml} + + 3 0 8 0 0 0 + + + + 1 1 1 + + + 0.8 0.2 0.2 1 + 1.0 0 0 1 + + + + + 1 1 1 + + + + +\endcode + +For now, the model will only contain a single link with a `` and a ``. Next, we create a model named "trigger" that contains the contact sensor, the TouchPlugin and DetachableJoint systems as well as visuals indicating where the sensor is on the ground. + +\code{.xml} + + + 3 0 0 0 0 0 + true + + + + 0.1 10 0.01 + + + + + 0.1 10 0.01 + + + + + c1 + + + + + vehicle_blue + trigger + + true + + + body + box1 + box_body + /box1/detach + + + +\endcode + +\note The contact sensor needs the `Contact` system under `` +\code{.xml} + + ... + + + ... + +\endcode + +The `DetachableJoint` system creates a fixed joint between the link "body" in `trigger` and the link "box_body" in `box1`. +The model `trigger` is a static model, hence, `box1` will remain fixed in space as long as it is attached to `trigger`. The `DetachableJoint` system subscribes to the `/box1/detach` topic and, on receipt of a message, will break the fixed joint and lets `box1` fall to the ground. + +When the vehicle runs over the contact sensor associated with `c1`, the TouchPlugin will publish a message on `/trigger/touched`. We will use this as our trigger to send a message to `/box1/detach`. The TouchPlugin publishes only when there is contact, so we can trigger on any received message. However, to demonstrate the use of matchers, we will only trigger when the Boolean input message is `true` + +\code{.xml} + + + data: true + + + +\endcode + +Finally, we will use an Altimeter sensor to detect when `box1` has fallen to the ground to cause another box to fall. We will add the Altimeter sensor to the link "box_body" in `box1` + +\code{.xml} + + + 3 0 8 0 0 0 + + ... + + /altimeter + 1 + 30 + true + + + +\endcode + +\note The Altimeter sensor needs the `Altimter` system under `` +\code{.xml} + + ... + + + ... + +\endcode + +We will call the second falling box `box2` and it will contain the same types of visuals and collisions as in box1. +\code{.xml} + + 5 0 8 0 0 0 + ... + +\endcode + +Again, we'll make use of the `DetachableJoint` system to attach `box2` to the static model `trigger` by adding the following to `trigger` + +\code{.xml} + + ... + + body + box2 + box_body + /box2/detach + + +\endcode + +Similar to what we did for `box1`, we need to publish to `/box2/detach` when our desired trigger occurs. To setup our trigger, we observe that the altimeter publishes an `ignition.msgs.Altimeter` message that contains a `vertical_position` field. Since we do not necessarily care about the values of the other fields inside `ignition.msgs.Altimeter`, we will create a TriggeredPublisher matcher that matches a specific field. + +The value of the `vertical_position` field will be the altitude of the link that it is associated with relative to its starting altitude. When `box1` falls to the ground, the value of the altimeter will read about -7.5. However, since we do not know the exact value and an exact comparison of floating point numbers is not advised, we will set a tolerance of 0.2. + +\code{.xml} + + + -7.5 + + + +\endcode + +We can now run the simulation and from the command line publish the start message +``` +ign topic -t "/start" -m ignition.msgs.Empty -p " " +``` From ac85dee61908a6269f945bd0dde05d4713994452 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 19 May 2020 19:48:33 -0500 Subject: [PATCH 15/23] Update changelog Signed-off-by: Addisu Z. Taddese --- Changelog.md | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/Changelog.md b/Changelog.md index 4cd4a50ee5..9c5160fbc5 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,14 +2,17 @@ ### Ignition Gazebo 2.xx.xx (2020-xx-xx) +1. Add TriggeredPublisher system + * [Pull Request 120](https://github.com/ignitionrobotics/ign-gazebo/pull/139) + +1. DetachableJoint system: Add option to suppress warning about missing child model + * [Pull Request 132](https://github.com/ignitionrobotics/ign-gazebo/pull/132) + ### Ignition Gazebo 2.17.0 (2020-05-13) 1. Allow battery plugin to work with joint force systems. * [Pull Request 120](https://github.com/ignitionrobotics/ign-gazebo/pull/120) -1. DetachableJoint system: Add option to suppress warning about missing child model - * [Pull Request 132](https://github.com/ignitionrobotics/ign-gazebo/pull/132) - 1. Make breadcrumb static after specified time * [Pull Request 90](https://github.com/ignitionrobotics/ign-gazebo/pull/90) From 065ebe47149b42a5a37f1468d71baba38afb7fdc Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Wed, 20 May 2020 06:03:24 -0700 Subject: [PATCH 16/23] Support multi entity spawn (#146) * Support multi entity spawn Signed-off-by: Nate Koenig * Added more documentation Signed-off-by: Nate Koenig Co-authored-by: Nate Koenig Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 5 +++- src/systems/user_commands/UserCommands.cc | 32 +++++++++++++++++++++++ src/systems/user_commands/UserCommands.hh | 9 +++++++ 4 files changed, 46 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1ad7e9ed65..c909d42a13 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo2 VERSION 2.17.0) +project(ignition-gazebo2 VERSION 2.18.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 9c5160fbc5..9b8a93d486 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,9 @@ ## Ignition Gazebo 2.x -### Ignition Gazebo 2.xx.xx (2020-xx-xx) +### 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. + * [Pull Request 146](https://github.com/ignitionrobotics/ign-gazebo/pull/146) 1. Add TriggeredPublisher system * [Pull Request 120](https://github.com/ignitionrobotics/ign-gazebo/pull/139) diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index c41749532e..a3ad13250c 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -157,6 +157,14 @@ class ignition::gazebo::systems::UserCommandsPrivate public: bool CreateService(const msgs::EntityFactory &_req, msgs::Boolean &_res); + /// \brief Callback for multiple create service + /// \param[in] _req Request containing one or more entity descriptions. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the entities will be successfully spawned. + /// \return True if successful. + public: bool CreateServiceMultiple( + const msgs::EntityFactory_V &_req, msgs::Boolean &_res); + /// \brief Callback for remove service /// \param[in] _req Request containing identification of entity to be removed. /// \param[in] _res True if message successfully received and queued. @@ -215,6 +223,11 @@ void UserCommands::Configure(const Entity &_entity, this->dataPtr->node.Advertise(createService, &UserCommandsPrivate::CreateService, this->dataPtr.get()); + // Create service for EntityFactory_V + std::string createServiceMultiple{"/world/" + worldName + "/create_multiple"}; + this->dataPtr->node.Advertise(createServiceMultiple, + &UserCommandsPrivate::CreateServiceMultiple, this->dataPtr.get()); + ignmsg << "Create service on [" << createService << "]" << std::endl; // Remove service @@ -265,6 +278,25 @@ void UserCommands::PreUpdate(const UpdateInfo &/*_info*/, // TODO(louise) Clear redo list } +////////////////////////////////////////////////// +bool UserCommandsPrivate::CreateServiceMultiple( + const msgs::EntityFactory_V &_req, msgs::Boolean &_res) +{ + std::lock_guard lock(this->pendingMutex); + for (int i = 0; i < _req.data_size(); ++i) + { + const msgs::EntityFactory &msg = _req.data(i); + // Create command and push it to queue + auto msgCopy = msg.New(); + msgCopy->CopyFrom(msg); + auto cmd = std::make_unique(msgCopy, this->iface); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::CreateService(const msgs::EntityFactory &_req, msgs::Boolean &_res) diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh index 0d5ea4cddf..d7f659eeae 100644 --- a/src/systems/user_commands/UserCommands.hh +++ b/src/systems/user_commands/UserCommands.hh @@ -45,6 +45,15 @@ namespace systems /// * **Request type*: ignition.msgs.EntityFactory /// * **Response type*: ignition.msgs.Boolean /// + /// # Spawn multiple entities + /// + /// This service can spawn multiple entities in the same iteration, + /// thereby eliminating simulation steps between entity spawn times. + /// + /// * **Service**: `/world//create_multiple` + /// * **Request type*: ignition.msgs.EntityFactory_V + /// * **Response type*: ignition.msgs.Boolean + /// /// Try some examples described on examples/worlds/empty.sdf class IGNITION_GAZEBO_VISIBLE UserCommands: public System, From 095c0e1bca720046dad9c214810f15704b932ae5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 20 May 2020 10:29:25 -0700 Subject: [PATCH 17/23] Bump ign-msgs version to 4.9 (#148) Signed-off-by: Louise Poubel Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c909d42a13..bb886cc186 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,7 +52,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport7_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs4 REQUIRED VERSION 4.8) +ign_find_package(ignition-msgs4 REQUIRED VERSION 4.9) set(IGN_MSGS_VER ${ignition-msgs4_VERSION_MAJOR}) #-------------------------------------- From dff4f30072656668f53d783432a1fef3bd716af6 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 21 May 2020 21:11:50 -0500 Subject: [PATCH 18/23] Update changelog Signed-off-by: Addisu Z. Taddese --- Changelog.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/Changelog.md b/Changelog.md index 9b8a93d486..a62f7dd2e1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,13 +1,15 @@ ## Ignition Gazebo 2.x +### Ignition Gazebo 2.XX.XX (20XX-XX-XX) + +1. Add TriggeredPublisher system + * [Pull Request 139](https://github.com/ignitionrobotics/ign-gazebo/pull/139) + ### 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. * [Pull Request 146](https://github.com/ignitionrobotics/ign-gazebo/pull/146) -1. Add TriggeredPublisher system - * [Pull Request 120](https://github.com/ignitionrobotics/ign-gazebo/pull/139) - 1. DetachableJoint system: Add option to suppress warning about missing child model * [Pull Request 132](https://github.com/ignitionrobotics/ign-gazebo/pull/132) From 7f1cfaeb0d96fda9124baed5bfb25b6c00da1227 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 21 May 2020 21:20:25 -0500 Subject: [PATCH 19/23] Fix example instructions and describe the expected result Signed-off-by: Addisu Z. Taddese --- examples/worlds/triggered_publisher.sdf | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 01f7fa79a9..7bf68b12b8 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -3,7 +3,12 @@ Ignition Gazebo Trigger Publisher demo Send the command: - ign topic -t "/start" -m ignition.msgs.Twist -p " " + ign topic -t "/start" -m ignition.msgs.Empty -p " " + +The vehicle should start moving while the two boxes remain floating. When the +vehicle crosses the line on the ground, the first box should start falling. The +moment the box hits the ground, the second box should start falling. + --> From 870ed8ba8486177635072ef07b17cc17303e7e7a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Thu, 21 May 2020 21:53:25 -0500 Subject: [PATCH 20/23] Address reviewer feedback Signed-off-by: Addisu Z. Taddese --- .../triggered_publisher/TriggeredPublisher.cc | 2 +- tutorials.md.in | 2 +- tutorials/triggered_publisher.md | 73 ++++++++++++++----- 3 files changed, 55 insertions(+), 22 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index a8598edbb1..356b4ab4e3 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -187,7 +187,7 @@ InputMatcher::InputMatcher(const std::string &_msgType) ////////////////////////////////////////////////// bool InputMatcher::Match(const transport::ProtoMsg &_input) const { - if (!CheckTypeMatch(*this->matchMsg, _input)) + if (!this->CheckTypeMatch(*this->matchMsg, _input)) { return false; } diff --git a/tutorials.md.in b/tutorials.md.in index 8be419a777..d86a39bea8 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -17,7 +17,7 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage battery "Battery": Keep track of battery charge on robot models * \subpage debugging "Debugging": Information about debugging Gazebo. * \subpage detachablejoints "Detachable Joints": Creating models that start off rigidly attached and then get detached during simulation -* \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate events in simulation +* \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation ## License diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 1d0b83430f..84bc030ec4 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -1,6 +1,6 @@ \page triggeredpublisher Triggered Publisher -The TriggeredPublisher system publishes a user specified message on an output +The `TriggeredPublisher` system publishes a user specified message on an output topic in response to an input message that matches user specified criteria. The system works by checking the input against a set of Matchers. Matchers contain string representations of protobuf messages which are compared for @@ -12,12 +12,14 @@ cause a box to fall from its initial position by detaching a detachable joint in response to the motion of a vehicle. The tutorial also covers how Triggered Publisher systems can be chained together by showing how the falling of the box can trigger another box to fall. The finished world SDFormat file for this -tutorial can be found in `examples/worlds/triggered_publisher.sdf` +tutorial can be found in +[examples/worlds/triggered_publisher.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/triggered_publisher.sdf) We will use the differential drive vehicle from -`/examples/worlds/diff_drive.sdf`, but modify the input topic of the -`DiffDrive` system to `cmd_vel`. A snippet of the change to the `DiffDrive` -system is shown below: +[examples/worlds/diff_drive.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/diff_drive.sdf), +but modify the input topic of the `DiffDrive` system to `cmd_vel`. A snippet of +the change to the `DiffDrive` system is shown below: + \code{.xml} ... @@ -36,7 +38,7 @@ system is shown below: \endcode -The first TriggeredPublisher we create will demonstrate how we can send +The first `TriggeredPublisher` we create will demonstrate how we can send a predetermined `Twist` message to the `DiffDrive` vehicle in response to a "start" message from the user: @@ -49,18 +51,19 @@ a "start" message from the user: \endcode -The `` tag sets up the TriggeredPublisher to subscribe to the topic +The `` tag sets up the `TriggeredPublisher` to subscribe to the topic `/start` with a message type of `ignition.msgs.Empty`. The `` tag specifies the topic of the output and the actual data to be published. The data is expressed in the human-readable form of Google Protobuf messages. This is the same format used by `ign topic` for publishing messages. -Since the TriggeredPublisher only deals with ignition topics, it can be +Since the `TriggeredPublisher` only deals with Ignition topics, it can be anywhere a `` tag is allowed. For this example, we will put it under ``. -Next we will create a trigger that causes a box to fall when the `DiffDrive` vehicle crosses a contact sensor on the ground. To do this, -we first create the falling box model and call it `box1` +Next we will create a trigger that causes a box to fall when the `DiffDrive` +vehicle crosses a contact sensor on the ground. To do this, we first create the +falling box model and call it `box1` \code{.xml} @@ -84,7 +87,10 @@ we first create the falling box model and call it `box1` \endcode -For now, the model will only contain a single link with a `` and a ``. Next, we create a model named "trigger" that contains the contact sensor, the TouchPlugin and DetachableJoint systems as well as visuals indicating where the sensor is on the ground. +For now, the model will only contain a single link with a `` and a +``. Next, we create a model named "trigger" that contains the +contact sensor, the `TouchPlugin` and `DetachableJoint` systems as well as visuals +indicating where the sensor is on the ground. \code{.xml} @@ -138,10 +144,19 @@ For now, the model will only contain a single link with a `` and a ` \endcode -The `DetachableJoint` system creates a fixed joint between the link "body" in `trigger` and the link "box_body" in `box1`. -The model `trigger` is a static model, hence, `box1` will remain fixed in space as long as it is attached to `trigger`. The `DetachableJoint` system subscribes to the `/box1/detach` topic and, on receipt of a message, will break the fixed joint and lets `box1` fall to the ground. +The `DetachableJoint` system creates a fixed joint between the link "body" in +`trigger` and the link "box_body" in `box1`. The model `trigger` is a static +model, hence, `box1` will remain fixed in space as long as it is attached to +`trigger`. The `DetachableJoint` system subscribes to the `/box1/detach` topic +and, on receipt of a message, will break the fixed joint and lets `box1` fall +to the ground. -When the vehicle runs over the contact sensor associated with `c1`, the TouchPlugin will publish a message on `/trigger/touched`. We will use this as our trigger to send a message to `/box1/detach`. The TouchPlugin publishes only when there is contact, so we can trigger on any received message. However, to demonstrate the use of matchers, we will only trigger when the Boolean input message is `true` +When the vehicle runs over the contact sensor associated with `c1`, the +`TouchPlugin` will publish a message on `/trigger/touched`. We will use this as +our trigger to send a message to `/box1/detach`. The `TouchPlugin` publishes +only when there is contact, so we can trigger on any received message. However, +to demonstrate the use of matchers, we will only trigger when the Boolean input +message is `true` \code{.xml} @@ -152,7 +167,9 @@ When the vehicle runs over the contact sensor associated with `c1`, the TouchPlu \endcode -Finally, we will use an Altimeter sensor to detect when `box1` has fallen to the ground to cause another box to fall. We will add the Altimeter sensor to the link "box_body" in `box1` +Finally, we will use an Altimeter sensor to detect when `box1` has fallen to +the ground to cause another box to fall. We will add the Altimeter sensor to +the link "box_body" in `box1` \code{.xml} @@ -181,7 +198,8 @@ Finally, we will use an Altimeter sensor to detect when `box1` has fallen to the \endcode -We will call the second falling box `box2` and it will contain the same types of visuals and collisions as in box1. +We will call the second falling box `box2` and it will contain the same types +of visuals and collisions as in box1. \code{.xml} 5 0 8 0 0 0 @@ -189,7 +207,8 @@ We will call the second falling box `box2` and it will contain the same types of \endcode -Again, we'll make use of the `DetachableJoint` system to attach `box2` to the static model `trigger` by adding the following to `trigger` +Again, we'll make use of the `DetachableJoint` system to attach `box2` to the +static model `trigger` by adding the following to `trigger` \code{.xml} @@ -203,9 +222,18 @@ Again, we'll make use of the `DetachableJoint` system to attach `box2` to the st \endcode -Similar to what we did for `box1`, we need to publish to `/box2/detach` when our desired trigger occurs. To setup our trigger, we observe that the altimeter publishes an `ignition.msgs.Altimeter` message that contains a `vertical_position` field. Since we do not necessarily care about the values of the other fields inside `ignition.msgs.Altimeter`, we will create a TriggeredPublisher matcher that matches a specific field. +Similar to what we did for `box1`, we need to publish to `/box2/detach` when +our desired trigger occurs. To setup our trigger, we observe that the altimeter +publishes an `ignition.msgs.Altimeter` message that contains a +`vertical_position` field. Since we do not necessarily care about the values of +the other fields inside `ignition.msgs.Altimeter`, we will create a +`TriggeredPublisher` matcher that matches a specific field. -The value of the `vertical_position` field will be the altitude of the link that it is associated with relative to its starting altitude. When `box1` falls to the ground, the value of the altimeter will read about -7.5. However, since we do not know the exact value and an exact comparison of floating point numbers is not advised, we will set a tolerance of 0.2. +The value of the `vertical_position` field will be the altitude of the link +that it is associated with relative to its starting altitude. When `box1` falls +to the ground, the value of the altimeter will read about -7.5. However, since +we do not know the exact value and an exact comparison of floating point +numbers is not advised, we will set a tolerance of 0.2. \code{.xml} @@ -216,7 +244,12 @@ The value of the `vertical_position` field will be the altitude of the link that \endcode -We can now run the simulation and from the command line publish the start message +We can now run the simulation and from the command line by running +``` +ign gazebo -r triggered_publisher.sdf +``` + +and publish the start message ``` ign topic -t "/start" -m ignition.msgs.Empty -p " " ``` From e56d0278414ff9aba2072fabf2ea2d703852e248 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 22 May 2020 10:47:08 -0500 Subject: [PATCH 21/23] Improve github preview Signed-off-by: Addisu Z. Taddese --- tutorials/triggered_publisher.md | 191 ++++++++++++++++--------------- 1 file changed, 100 insertions(+), 91 deletions(-) diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 84bc030ec4..25e7e4148d 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -12,44 +12,46 @@ cause a box to fall from its initial position by detaching a detachable joint in response to the motion of a vehicle. The tutorial also covers how Triggered Publisher systems can be chained together by showing how the falling of the box can trigger another box to fall. The finished world SDFormat file for this -tutorial can be found in +tutorial can be found in [examples/worlds/triggered_publisher.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/triggered_publisher.sdf) We will use the differential drive vehicle from -[examples/worlds/diff_drive.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/diff_drive.sdf), -but modify the input topic of the `DiffDrive` system to `cmd_vel`. A snippet of +[examples/worlds/diff_drive.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/diff_drive.sdf), +but modify the input topic of the `DiffDrive` system to `cmd_vel`. A snippet of the change to the `DiffDrive` system is shown below: -\code{.xml} - - ... +```xml + + ... - - front_left_wheel_joint - rear_left_wheel_joint - front_right_wheel_joint - rear_right_wheel_joint - 1.25 - 0.3 - cmd_vel - - -\endcode + + front_left_wheel_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 1.25 + 0.3 + cmd_vel + + + +``` The first `TriggeredPublisher` we create will demonstrate how we can send a predetermined `Twist` message to the `DiffDrive` vehicle in response to a "start" message from the user: -\code{.xml} - +```xml + linear: {x: 3} -\endcode +``` The `` tag sets up the `TriggeredPublisher` to subscribe to the topic `/start` with a message type of `ignition.msgs.Empty`. The `` tag @@ -61,11 +63,11 @@ Since the `TriggeredPublisher` only deals with Ignition topics, it can be anywhere a `` tag is allowed. For this example, we will put it under ``. -Next we will create a trigger that causes a box to fall when the `DiffDrive` -vehicle crosses a contact sensor on the ground. To do this, we first create the +Next we will create a trigger that causes a box to fall when the `DiffDrive` +vehicle crosses a contact sensor on the ground. To do this, we first create the falling box model and call it `box1` -\code{.xml} +```xml 3 0 8 0 0 0 @@ -85,14 +87,14 @@ falling box model and call it `box1` -\endcode +``` -For now, the model will only contain a single link with a `` and a -``. Next, we create a model named "trigger" that contains the -contact sensor, the `TouchPlugin` and `DetachableJoint` systems as well as visuals +For now, the model will only contain a single link with a `` and a +``. Next, we create a model named "trigger" that contains the +contact sensor, the `TouchPlugin` and `DetachableJoint` systems as well as visuals indicating where the sensor is on the ground. -\code{.xml} +```xml 3 0 0 0 0 0 @@ -122,7 +124,8 @@ indicating where the sensor is on the ground. true - + body box1 box_body @@ -130,48 +133,49 @@ indicating where the sensor is on the ground. -\endcode +``` \note The contact sensor needs the `Contact` system under `` -\code{.xml} - - ... - - - ... - -\endcode - -The `DetachableJoint` system creates a fixed joint between the link "body" in -`trigger` and the link "box_body" in `box1`. The model `trigger` is a static -model, hence, `box1` will remain fixed in space as long as it is attached to -`trigger`. The `DetachableJoint` system subscribes to the `/box1/detach` topic -and, on receipt of a message, will break the fixed joint and lets `box1` fall -to the ground. - -When the vehicle runs over the contact sensor associated with `c1`, the -`TouchPlugin` will publish a message on `/trigger/touched`. We will use this as -our trigger to send a message to `/box1/detach`. The `TouchPlugin` publishes -only when there is contact, so we can trigger on any received message. However, -to demonstrate the use of matchers, we will only trigger when the Boolean input +```xml + + ... + + + ... + +``` + +The `DetachableJoint` system creates a fixed joint between the link "body" in +`trigger` and the link "box_body" in `box1`. The model `trigger` is a static +model, hence, `box1` will remain fixed in space as long as it is attached to +`trigger`. The `DetachableJoint` system subscribes to the `/box1/detach` topic +and, on receipt of a message, will break the fixed joint and lets `box1` fall +to the ground. + +When the vehicle runs over the contact sensor associated with `c1`, the +`TouchPlugin` will publish a message on `/trigger/touched`. We will use this as +our trigger to send a message to `/box1/detach`. The `TouchPlugin` publishes +only when there is contact, so we can trigger on any received message. However, +to demonstrate the use of matchers, we will only trigger when the Boolean input message is `true` -\code{.xml} - +```xml + data: true -\endcode +``` -Finally, we will use an Altimeter sensor to detect when `box1` has fallen to -the ground to cause another box to fall. We will add the Altimeter sensor to +Finally, we will use an Altimeter sensor to detect when `box1` has fallen to +the ground to cause another box to fall. We will add the Altimeter sensor to the link "box_body" in `box1` -\code{.xml} +```xml 3 0 8 0 0 0 @@ -185,71 +189,76 @@ the link "box_body" in `box1` -\endcode +``` \note The Altimeter sensor needs the `Altimter` system under `` -\code{.xml} - - ... - - - ... - -\endcode +```xml + + ... + + + ... + +``` -We will call the second falling box `box2` and it will contain the same types +We will call the second falling box `box2` and it will contain the same types of visuals and collisions as in box1. -\code{.xml} + +```xml 5 0 8 0 0 0 ... -\endcode +``` -Again, we'll make use of the `DetachableJoint` system to attach `box2` to the -static model `trigger` by adding the following to `trigger` +Again, we'll make use of the `DetachableJoint` system to attach `box2` to the +static model `trigger` by adding the following to `trigger` -\code{.xml} +```xml ... - + body box2 box_body /box2/detach -\endcode +``` -Similar to what we did for `box1`, we need to publish to `/box2/detach` when -our desired trigger occurs. To setup our trigger, we observe that the altimeter -publishes an `ignition.msgs.Altimeter` message that contains a -`vertical_position` field. Since we do not necessarily care about the values of -the other fields inside `ignition.msgs.Altimeter`, we will create a +Similar to what we did for `box1`, we need to publish to `/box2/detach` when +our desired trigger occurs. To setup our trigger, we observe that the altimeter +publishes an `ignition.msgs.Altimeter` message that contains a +`vertical_position` field. Since we do not necessarily care about the values of +the other fields inside `ignition.msgs.Altimeter`, we will create a `TriggeredPublisher` matcher that matches a specific field. -The value of the `vertical_position` field will be the altitude of the link -that it is associated with relative to its starting altitude. When `box1` falls -to the ground, the value of the altimeter will read about -7.5. However, since -we do not know the exact value and an exact comparison of floating point +The value of the `vertical_position` field will be the altitude of the link +that it is associated with relative to its starting altitude. When `box1` falls +to the ground, the value of the altimeter will read about -7.5. However, since +we do not know the exact value and an exact comparison of floating point numbers is not advised, we will set a tolerance of 0.2. -\code{.xml} - +```xml + -7.5 -\endcode +``` We can now run the simulation and from the command line by running + ``` ign gazebo -r triggered_publisher.sdf ``` and publish the start message + ``` ign topic -t "/start" -m ignition.msgs.Empty -p " " ``` From a66de0ba0ff83e9fcb88b6d169a45324c781fe39 Mon Sep 17 00:00:00 2001 From: Addisu Taddese Date: Fri, 22 May 2020 10:52:56 -0500 Subject: [PATCH 22/23] Fix typo Co-authored-by: Louise Poubel Signed-off-by: Addisu Z. Taddese --- src/systems/triggered_publisher/TriggeredPublisher.hh | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index 759683bf37..3109771915 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -56,7 +56,7 @@ namespace systems /// succeeds. A "negative" value triggers a match when a comparson /// fails. The default value is "positive" /// * `tol`: Tolerance for floating point comparisons. - /// * `field`: If specified, specified, only this field inside the input + /// * `field`: If specified, only this field inside the input /// message is compared for a match. /// * Value: String used to construct the protobuf message against which /// input messages are matched. This is the human-readable @@ -230,4 +230,3 @@ namespace systems } #endif - From f2fbb5e0dd184789fe848fe8e8437e3183af7081 Mon Sep 17 00:00:00 2001 From: Addisu Taddese Date: Fri, 22 May 2020 10:53:12 -0500 Subject: [PATCH 23/23] Fix style Co-authored-by: Louise Poubel Signed-off-by: Addisu Z. Taddese --- src/systems/triggered_publisher/TriggeredPublisher.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 356b4ab4e3..6754217f14 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -579,7 +579,7 @@ void TriggeredPublisher::Configure(const Entity &, { { std::lock_guard lock(this->publishCountMutex); - ++publishCount; + ++this->publishCount; } this->newMatchSignal.notify_one(); }