diff --git a/Changelog.md b/Changelog.md index a912f7399d..2f301c0a95 100644 --- a/Changelog.md +++ b/Changelog.md @@ -2,6 +2,9 @@ ### Ignition Gazebo 2.XX.XX (20XX-XX-XX) +1. Add TriggeredPublisher system + * [Pull Request 139](https://github.com/ignitionrobotics/ign-gazebo/pull/139) + 1. Add PerformerDetector, a system for detecting when performers enter a specified region * [Pull Request 125](https://github.com/ignitionrobotics/ign-gazebo/pull/125) @@ -10,14 +13,14 @@ 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. 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) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf new file mode 100644 index 0000000000..7bf68b12b8 --- /dev/null +++ b/examples/worlds/triggered_publisher.sdf @@ -0,0 +1,538 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 3 -9 9 0 0.6 1.3 + + + + + + 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.05 -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 0 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 + + + + + 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/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 87f6d11e8d..1180e75b91 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -94,5 +94,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..6754217f14 --- /dev/null +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -0,0 +1,662 @@ +/* + * 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 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: bool Match(const transport::ProtoMsg &_input) const; + + /// \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 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 + /// \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 + /// 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 Protobuf message for matching against input + protected: std::unique_ptr matchMsg; + + /// \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; +}; + +////////////////////////////////////////////////// +/// \brief Matches any input message of the specified type +class AnyMatcher : public InputMatcher +{ + /// \brief Constructor + /// \param[in] _msgType Input message type + public: explicit AnyMatcher(const std::string &_msgType); + + // Documentation inherited + public: bool DoMatch(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 publishing + /// messages + public: FullMatcher(const std::string &_msgType, bool _logicType, + const std::string &_matchString); + + // Documentation inherited + public: bool DoMatch(const transport::ProtoMsg &_input) const override; + + /// \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 publishing messages + public: FieldMatcher(const std::string &_msgType, bool _logicType, + const std::string &_fieldName, + const std::string &_fieldString); + + // Documentation inherited + 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 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; +}; + +////////////////////////////////////////////////// +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); +} + +////////////////////////////////////////////////// +bool InputMatcher::Match(const transport::ProtoMsg &_input) const +{ + if (!this->CheckTypeMatch(*this->matchMsg, _input)) + { + return false; + } + 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) +{ + 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) + : InputMatcher(_msgType), logicType(_logicType) +{ + if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + return; + + this->valid = google::protobuf::TextFormat::ParseFromString( + _matchString, this->matchMsg.get()); +} + +////////////////////////////////////////////////// +bool FullMatcher::DoMatch(const transport::ProtoMsg &_input) const +{ + return this->logicType == this->diff.Compare(*this->matchMsg, _input); +} + +////////////////////////////////////////////////// +FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, + const std::string &_fieldName, + const std::string &_fieldString) + : InputMatcher(_msgType), + logicType(_logicType), + fieldName(_fieldName) +{ + if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + return; + + transport::ProtoMsg *matcherSubMsg{nullptr}; + if (!FindFieldSubMessage(this->matchMsg.get(), _fieldName, + this->fieldDescMatcher, &matcherSubMsg)) + { + return; + } + + 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; + + 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; + } + + 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) + { + 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) + { + ignerr << "Field name [" << fieldNames[i] + << "] could not be found in message type [" + << fieldMsgType->full_name() << "].\n"; + return false; + } + + _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 + << "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(); + } + } + + return true; +} + +////////////////////////////////////////////////// +bool FieldMatcher::DoMatch( + const transport::ProtoMsg &_input) const +{ + google::protobuf::util::DefaultFieldComparator comp; + + 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) + { + auto *fieldDesc = this->fieldDescMatcher[i]; + if (fieldDesc->is_repeated()) + { + // 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 + { + subMsgMatcher = &matcherRefl->GetMessage(*subMsgMatcher, fieldDesc); + subMsgInput = &inputRefl->GetMessage(*subMsgInput, fieldDesc); + } + } + + return this->logicType == + this->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(_msgType); + } + + std::unique_ptr matcher{nullptr}; + + 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()) + { + 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 for input type [" << _msgType + << "] could not be created from:\n" + << inputMatchString << std::endl; + return nullptr; + } + + const auto tol = _matchElem->Get("tol", 1e-8).first; + matcher->SetTolerance(tol); + } + 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"); + 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")) + { + 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"); + if (info.msgType.empty()) + { + ignerr << "Output message type cannot be empty\n"; + continue; + } + info.topic = outputElem->Get("topic"); + if (info.topic.empty()) + { + ignerr << "Output topic cannot be empty\n"; + continue; + } + 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.msgData->GetTypeName()); + 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 + { + ignerr << "Unable to create message of type [" << info.msgType + << "] with data [" << msgStr << "] when creating output" + << " publisher on topic " << info.topic << ".\n"; + } + } + } + else + { + ignerr << "No ouptut specified" << std::endl; + return; + } + + auto msgCb = std::function( + [this](const auto &_msg) + { + if (this->MatchInput(_msg)) + { + { + std::lock_guard lock(this->publishCountMutex); + ++this->publishCount; + } + this->newMatchSignal.notify_one(); + } + }); + 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 + << " 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) + { + try + { + return _matcher->Match(_inputMsg); + } catch (const google::protobuf::FatalException &err) + { + ignerr << err.what() << std::endl; + return false; + } + }); +} + +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..3109771915 --- /dev/null +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -0,0 +1,232 @@ +/* + * 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. The default value is "positive" + /// * `tol`: Tolerance for floating point comparisons. + /// * `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 + /// representation of a protobuf message as used by `ign topic` for + /// publishing 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 publishing messages + /// + /// Examples: + /// 1. Any receipt of a Boolean messages on the input topic triggers an output + /// \code{.xml} + /// + /// + /// + /// + /// \endcode + /// + /// 2. Full match: An output is triggered when a Boolean message with a value + /// of "true" is received + /// \code{.xml} + /// + /// + /// + /// data: true + /// + /// + /// + /// + /// \endcode + /// + /// 3. Field match: An output is triggered when a specific field matches + /// \code{.xml} + /// + /// + /// 1.0 + /// 2.0 + /// + /// + /// + /// \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 + /// 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 + { + /// \brief Constructor + public: TriggeredPublisher() = default; + + /// \brief Destructor + public: ~TriggeredPublisher() override; + + // 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 8b13f15799..87a1df7da9 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -33,6 +33,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..70230ba09e --- /dev/null +++ b/test/integration/triggered_publisher.cc @@ -0,0 +1,587 @@ +/* + * 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; +}; + +/// \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 +/// configuration +TEST_F(TriggeredPublisherTest, EmptyInputEmptyOutput) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_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())); + } + waitUntil(5000, [&]{return pubCount == recvCount;}); + + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, WrongInputMessageTypeDoesNotMatch) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_0"); + std::atomic 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())); + } + + waitUntil(5000, [&]{return 0u == recvCount;}); + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, InputMessagesTriggerOutputs) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_1"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &_msg) + { + EXPECT_TRUE(_msg.data()); + ++recvCount; + }); + node.Subscribe("/out_1", msgCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(10); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, MultipleOutputsForOneInput) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_2"); + std::mutex recvMsgMutex; + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) + { + return std::function( + [&_msgVector, &recvMsgMutex](const msgs::Boolean &_msg) + { + std::lock_guard lock(recvMsgMutex); + _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) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + 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)); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, ExactMatchBooleanInputs) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_3"); + std::atomic 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) + { + EXPECT_TRUE(inputPub.Publish(msgs::Convert(true))); + } + else + { + EXPECT_TRUE(inputPub.Publish(msgs::Convert(false))); + } + IGN_SLEEP_MS(10); + } + + // 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, MatchersWithLogicTypeAttribute) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_4"); + std::atomic recvCount[2]{0, 0}; + + auto cbCreator = [](std::atomic &_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) + { + EXPECT_TRUE(inputPub.Publish( + msgs::Convert(static_cast(i - pubCount / 2)))); + IGN_SLEEP_MS(10); + } + // 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]); +} + +///////////////////////////////////////////////// +TEST_F(TriggeredPublisherTest, MultipleMatchersAreAnded) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_5"); + std::atomic 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) + { + EXPECT_TRUE(inputPub.Publish( + msgs::Convert(static_cast(i - pubCount / 2)))); + IGN_SLEEP_MS(10); + } + // 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::atomic recvCount[2]{0, 0}; + + auto cbCreator = [](std::atomic &_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)); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + // 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::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_7", msgCb); + + 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"); + EXPECT_TRUE(inputPub.Publish(poseMsg)); + IGN_SLEEP_MS(10); + } + + // 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); +} + +TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedSubFieldExpected) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_7"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_7", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Empty msg; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + EXPECT_EQ(0u, 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_8"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_8", msgCb); + IGN_SLEEP_MS(10); + + 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("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)); + IGN_SLEEP_MS(10); + } + + // 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); +} + +///////////////////////////////////////////////// +/// 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::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_9", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Int32_V msg; + for (int i = 0; i < pubCount; ++i) + { + msg.add_data(i); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + // The input contains an increasing sets of sequences, {0}, {0,1}, {0,1,2}... + // The matcher only matches {0,1} + EXPECT_EQ(1u, recvCount); +} + +TEST_F(TriggeredPublisherTest, FullMatchersAcceptToleranceParam) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_10"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_10", msgCb); + IGN_SLEEP_MS(10); + + 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)); + IGN_SLEEP_MS(10); + } + + // 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::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_11", msgCb); + IGN_SLEEP_MS(10); + + 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)); + IGN_SLEEP_MS(10); + } + + // 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, SubfieldsOfRepeatedFieldsNotSupported) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_12"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_12", msgCb); + IGN_SLEEP_MS(10); + + 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)); + IGN_SLEEP_MS(10); + } + + // Subfields of repeated fiealds are not supported, so no output should be + // triggered. + EXPECT_EQ(0u, recvCount); +} + +TEST_F(TriggeredPublisherTest, WrongInputWhenRepeatedFieldExpected) +{ + transport::Node node; + auto inputPub = node.Advertise("/invalid_topic"); + std::atomic recvCount{0}; + auto msgCb = std::function( + [&recvCount](const auto &) + { + ++recvCount; + }); + node.Subscribe("/out_9", msgCb); + IGN_SLEEP_MS(10); + + const int pubCount{10}; + msgs::Int32 msg; + for (int i = 0; i < pubCount; ++i) + { + msg.set_data(i); + EXPECT_TRUE(inputPub.Publish(msg)); + IGN_SLEEP_MS(10); + } + + EXPECT_EQ(0u, recvCount); +} diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf new file mode 100644 index 0000000000..624d4c94b5 --- /dev/null +++ b/test/worlds/triggered_publisher.sdf @@ -0,0 +1,162 @@ + + + + + + + + + + + + data: true + + + + + + + data: false + + + data: true + + + + + + + data: true + + + + + + + + + data: 0 + + + + + + + + + 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 + + + + + + + data: 0.5 + + + + + + + 0.5 + + + + + + + "value1" + + + + + + + + data: 0, data: 1 + + + + + + + + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index 34586f8a08..d86a39bea8 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 actions in simulation ## License diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md new file mode 100644 index 0000000000..25e7e4148d --- /dev/null +++ b/tutorials/triggered_publisher.md @@ -0,0 +1,264 @@ +\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](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 +the change to the `DiffDrive` system is shown below: + +```xml + + ... + + + 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: + +```xml + + + + linear: {x: 3} + + +``` + +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` + +```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 + + + + +``` + +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. + +```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 + + + +``` + +\note The contact sensor needs the `Contact` system under `` +```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` + +```xml + + + data: true + + + +``` + +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` + +```xml + + + 3 0 8 0 0 0 + + ... + + /altimeter + 1 + 30 + true + + + +``` + +\note The Altimeter sensor needs the `Altimter` system under `` +```xml + + ... + + + ... + +``` + +We will call the second falling box `box2` and it will contain the same types +of visuals and collisions as in box1. + +```xml + + 5 0 8 0 0 0 + ... + +``` + +Again, we'll make use of the `DetachableJoint` system to attach `box2` to the +static model `trigger` by adding the following to `trigger` + +```xml + + ... + + body + box2 + box_body + /box2/detach + + +``` + +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. + +```xml + + + -7.5 + + + +``` + +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 " " +```