From 81a09131a487a44a02aef504d422424346b9e1d4 Mon Sep 17 00:00:00 2001 From: LolaSegura <48759425+LolaSegura@users.noreply.github.com> Date: Mon, 19 Jul 2021 04:52:10 -0300 Subject: [PATCH] New teleop plugin implementation. (#245) Signed-off-by: LolaSegura Signed-off-by: Franco Cipollone Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel Co-authored-by: Franco Cipollone --- src/plugins/CMakeLists.txt | 1 + src/plugins/teleop/CMakeLists.txt | 8 + src/plugins/teleop/Teleop.cc | 274 +++++++++++++++++++ src/plugins/teleop/Teleop.hh | 148 +++++++++++ src/plugins/teleop/Teleop.qml | 420 ++++++++++++++++++++++++++++++ src/plugins/teleop/Teleop.qrc | 5 + src/plugins/teleop/Teleop_TEST.cc | 355 +++++++++++++++++++++++++ 7 files changed, 1211 insertions(+) create mode 100644 src/plugins/teleop/CMakeLists.txt create mode 100644 src/plugins/teleop/Teleop.cc create mode 100644 src/plugins/teleop/Teleop.hh create mode 100644 src/plugins/teleop/Teleop.qml create mode 100644 src/plugins/teleop/Teleop.qrc create mode 100644 src/plugins/teleop/Teleop_TEST.cc diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index e5d6c9fb3..3a82d2945 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -120,6 +120,7 @@ add_subdirectory(key_publisher) add_subdirectory(publisher) add_subdirectory(scene3d) add_subdirectory(screenshot) +add_subdirectory(teleop) add_subdirectory(topic_echo) add_subdirectory(topic_viewer) add_subdirectory(world_control) diff --git a/src/plugins/teleop/CMakeLists.txt b/src/plugins/teleop/CMakeLists.txt new file mode 100644 index 000000000..a8ebc5bcc --- /dev/null +++ b/src/plugins/teleop/CMakeLists.txt @@ -0,0 +1,8 @@ +ign_gui_add_plugin(Teleop + SOURCES + Teleop.cc + QT_HEADERS + Teleop.hh + TEST_SOURCES + Teleop_TEST.cc +) diff --git a/src/plugins/teleop/Teleop.cc b/src/plugins/teleop/Teleop.cc new file mode 100644 index 000000000..1af41287f --- /dev/null +++ b/src/plugins/teleop/Teleop.cc @@ -0,0 +1,274 @@ +/* + * Copyright (C) 2021 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 +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include "Teleop.hh" + +#include + +#include + +#include +#include + +namespace ignition +{ +namespace gui +{ +namespace plugins +{ + enum class KeyLinear{ + kForward, + kBackward, + kStop, + }; + + enum class KeyAngular{ + kLeft, + kRight, + kStop, + }; + + class TeleopPrivate + { + /// \brief Node for communication. + public: ignition::transport::Node node; + + /// \brief Topic. Set '/cmd_vel' as default. + public: std::string topic = "/cmd_vel"; + + /// \brief Publisher. + public: ignition::transport::Node::Publisher cmdVelPub; + + /// \brief Linear velocity. + public: double linearVel = 0; + /// \brief Angular velocity. + public: double angularVel = 0; + + /// \brief Linear direction. + public: int linearDir = 0; + /// \brief Angular direction. + public: int angularDir = 0; + + /// \brief Linear state setted by keyboard input. + public: KeyLinear linearState = KeyLinear::kStop; + /// \brief Angular state setted by keyboard input. + public: KeyAngular angularState = KeyAngular::kStop; + + /// \brief Indicates if the keyboard is enabled or + /// disabled. + public: bool keyEnable = false; + }; +} +} +} + +using namespace ignition; +using namespace gui; +using namespace plugins; + +///////////////////////////////////////////////// +Teleop::Teleop(): Plugin(), dataPtr(std::make_unique()) +{ + // Initialize publisher using default topic. + this->dataPtr->cmdVelPub = ignition::transport::Node::Publisher(); + this->dataPtr->cmdVelPub = + this->dataPtr->node.Advertise + (this->dataPtr->topic); +} + +///////////////////////////////////////////////// +Teleop::~Teleop() = default; + +///////////////////////////////////////////////// +void Teleop::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Teleop"; + + ignition::gui::App()->findChild + ()->QuickWindow()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void Teleop::OnTeleopTwist() +{ + ignition::msgs::Twist cmdVelMsg; + + cmdVelMsg.mutable_linear()->set_x( + this->dataPtr->linearDir * this->dataPtr->linearVel); + cmdVelMsg.mutable_angular()->set_z( + this->dataPtr->angularDir * this->dataPtr->angularVel); + + if (!this->dataPtr->cmdVelPub.Publish(cmdVelMsg)) + ignerr << "ignition::msgs::Twist message couldn't be published at topic: " + << this->dataPtr->topic << std::endl; +} + +///////////////////////////////////////////////// +void Teleop::OnTopicSelection(const QString &_topic) +{ + this->dataPtr->topic = _topic.toStdString(); + ignmsg << "A new topic has been entered: '" << + this->dataPtr->topic << " ' " <dataPtr->cmdVelPub = ignition::transport::Node::Publisher(); + this->dataPtr->cmdVelPub = + this->dataPtr->node.Advertise + (this->dataPtr->topic); + if(!this->dataPtr->cmdVelPub) + ignerr << "Error when advertising topic: " << + this->dataPtr->topic << std::endl; +} + +///////////////////////////////////////////////// +void Teleop::OnLinearVelSelection(double _velocity) +{ + this->dataPtr->linearVel = _velocity; +} + +///////////////////////////////////////////////// +void Teleop::OnAngularVelSelection(double _velocity) +{ + this->dataPtr->angularVel = _velocity; +} + +///////////////////////////////////////////////// +void Teleop::OnKeySwitch(bool _checked) +{ + this->dataPtr->linearDir = 0; + this->dataPtr->angularDir = 0; + this->dataPtr->keyEnable = _checked; +} + +///////////////////////////////////////////////// +void Teleop::OnSlidersSwitch(bool _checked) +{ + if(_checked) + { + this->dataPtr->linearDir = 1; + this->dataPtr->angularDir = 1; + this->OnTeleopTwist(); + } +} + +///////////////////////////////////////////////// +bool Teleop::eventFilter(QObject *_obj, QEvent *_event) +{ + if(this->dataPtr->keyEnable == true) + { + if(_event->type() == QEvent::KeyPress) + { + QKeyEvent *keyEvent = static_cast(_event); + switch(keyEvent->key()) + { + case Qt::Key_W: + this->dataPtr->linearState = KeyLinear::kForward; + break; + case Qt::Key_A: + this->dataPtr->angularState = KeyAngular::kLeft; + break; + case Qt::Key_D: + this->dataPtr->angularState = KeyAngular::kRight; + break; + case Qt::Key_S: + this->dataPtr->linearState = KeyLinear::kBackward; + break; + default: + break; + } + this->SetKeyDirection(); + this->OnTeleopTwist(); + } + + if(_event->type() == QEvent::KeyRelease) + { + QKeyEvent *keyEvent = static_cast(_event); + switch(keyEvent->key()) + { + case Qt::Key_W: + this->dataPtr->linearState = KeyLinear::kStop; + break; + case Qt::Key_A: + this->dataPtr->angularState = KeyAngular::kStop; + break; + case Qt::Key_D: + this->dataPtr->angularState = KeyAngular::kStop; + break; + case Qt::Key_S: + this->dataPtr->linearState = KeyLinear::kStop; + break; + default: + break; + } + this->SetKeyDirection(); + this->OnTeleopTwist(); + } + } + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void Teleop::SetKeyDirection() +{ + this->dataPtr->linearDir = this->dataPtr->linearState == + KeyLinear::kForward ? 1 : this->dataPtr->linearState == + KeyLinear::kBackward ? -1 : 0; + + this->dataPtr->angularDir = this->dataPtr->angularState == + KeyAngular::kLeft ? 1 : this->dataPtr->angularState == + KeyAngular::kRight ? -1 : 0; +} + +///////////////////////////////////////////////// +int Teleop::LinearDirection() const +{ + return this->dataPtr->linearDir; +} + +///////////////////////////////////////////////// +void Teleop::setLinearDirection(int _linearDir) +{ + this->dataPtr->linearDir = _linearDir; + this->LinearDirectionChanged(); +} + +///////////////////////////////////////////////// +int Teleop::AngularDirection() const +{ + return this->dataPtr->angularDir; +} + +///////////////////////////////////////////////// +void Teleop::setAngularDirection(int _angularDir) +{ + this->dataPtr->angularDir = _angularDir; + this->AngularDirectionChanged(); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gui::plugins::Teleop, + ignition::gui::Plugin) diff --git a/src/plugins/teleop/Teleop.hh b/src/plugins/teleop/Teleop.hh new file mode 100644 index 000000000..187a77323 --- /dev/null +++ b/src/plugins/teleop/Teleop.hh @@ -0,0 +1,148 @@ +/* + * Copyright (C) 2021 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_GUI_PLUGINS_TELEOP_HH_ +#define IGNITION_GUI_PLUGINS_TELEOP_HH_ + +#include + +#include + +#include +#include + +#ifndef _WIN32 +# define Teleop_EXPORTS_API +#else +# if (defined(Teleop_EXPORTS)) +# define Teleop_EXPORTS_API __declspec(dllexport) +# else +# define Teleop_EXPORTS_API __declspec(dllimport) +# endif +#endif + +namespace ignition +{ +namespace gui +{ +namespace plugins +{ + class TeleopPrivate; + + /// \brief Publish teleop stokes to a user selected topic, + /// or to '/cmd_vel' if no topic is selected. + /// Buttons, the keyboard or sliders can be used to move a + /// vehicle load to the world. + /// ## Configuration + /// This plugin doesn't accept any custom configuration. + class Teleop_EXPORTS_API Teleop : public Plugin + { + Q_OBJECT + + /// \brief Linear direction + Q_PROPERTY( + int linearDir + READ LinearDirection + WRITE setLinearDirection + NOTIFY LinearDirectionChanged + ) + + /// \brief Angular direction + Q_PROPERTY( + int angularDir + READ AngularDirection + WRITE setAngularDirection + NOTIFY AngularDirectionChanged + ) + + /// \brief Constructor + public: Teleop(); + + /// \brief Destructor + public: virtual ~Teleop(); + + // Documentation inherited. + public: virtual void LoadConfig(const tinyxml2::XMLElement *) override; + + /// \brief Filters events of type 'keypress' and 'keyrelease'. + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \brief Publish the twist message to the selected command velocity topic. + public slots: void OnTeleopTwist(); + + /// \brief Returns the linear direction variable value. + /// When the movement is forward it takes the value 1, when + /// is backward it takes the value -1, and when it's 0 the + /// movement stops. + public: Q_INVOKABLE int LinearDirection() const; + + /// \brief Set the linear direction of the movement. + /// \param[in] _linearDir Modifier of the velocity for setting + /// the movement direction. + public: Q_INVOKABLE void setLinearDirection(int _linearDir); + + /// \brief Notify that the linear direction changed. + signals: void LinearDirectionChanged(); + + /// \brief Returns the angular direction variable value. + /// When the turn is counterclockwise it takes the value 1, when + /// is clockwise it takes the value -1, and when it's 0 the + /// movement stops. + public: Q_INVOKABLE int AngularDirection() const; + + /// \brief Set the angular direction of the movement. + /// \param[in] _angularDir Modifier of the velocity for setting + /// the direction of the rotation. + public: Q_INVOKABLE void setAngularDirection(int _angularDir); + + /// \brief Notify that the angular direction changed. + signals: void AngularDirectionChanged(); + + /// \brief Callback in Qt thread when the topic changes. + /// \param[in] _topic variable to indicate the topic to + /// publish the Twist commands. + public slots: void OnTopicSelection(const QString &_topic); + + /// \brief Callback in Qt thread when the linear velocity changes. + /// \param[in] _velocity variable to indicate the linear velocity. + public slots: void OnLinearVelSelection(double _velocity); + + /// \brief Callback in Qt thread when the angular velocity changes. + /// \param[in] _velocity variable to indicate the angular velocity. + public slots: void OnAngularVelSelection(double _velocity); + + /// \brief Callback in Qt thread when the keyboard is enabled or disabled. + /// \param[in] _checked variable to indicate the state of the switch. + public slots: void OnKeySwitch(bool _checked); + + /// \brief Callback in Qt thread when the sliders is enabled or disabled. + /// \param[in] _checked variable to indicate the state of the switch. + public slots: void OnSlidersSwitch(bool _checked); + + /// \brief Sets the movement direction when the keyboard is used. + public: void SetKeyDirection(); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + + }; +} +} +} + +#endif diff --git a/src/plugins/teleop/Teleop.qml b/src/plugins/teleop/Teleop.qml new file mode 100644 index 000000000..74f9eed9d --- /dev/null +++ b/src/plugins/teleop/Teleop.qml @@ -0,0 +1,420 @@ +/* + * Copyright (C) 2021 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.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Controls.Styles 1.4 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +Rectangle { + color:"transparent" + Layout.minimumWidth: 300 + Layout.minimumHeight: 900 + anchors.fill: parent + focus: true + + // Topic input + Label { + id: topicLabel + text: "Topic:" + anchors.top: parent.top + anchors.topMargin: 10 + anchors.left: parent.left + anchors.leftMargin: 5 + } + TextField { + id: topicField + anchors.top: topicLabel.bottom + anchors.topMargin: 5 + anchors.left: parent.left + anchors.leftMargin: 5 + Layout.fillWidth: true + text:"/cmd_vel" + placeholderText: qsTr("Topic to publish...") + onEditingFinished: { + Teleop.OnTopicSelection(text) + } + } + + // Velocity input + Label { + id: velocityLabel + text: "Velocity:" + anchors.top: topicField.bottom + anchors.topMargin: 10 + anchors.left: parent.left + anchors.leftMargin: 5 + } + // Linear velocity input + Label { + id: linearVelLabel + text: "Linear" + color: "dimgrey" + anchors.top: velocityLabel.bottom + anchors.topMargin: 15 + anchors.left: parent.left + anchors.leftMargin: 5 + } + IgnSpinBox { + id: linearVelField + anchors.top: velocityLabel.bottom + anchors.topMargin: 5 + anchors.left: linearVelLabel.right + anchors.leftMargin: 5 + Layout.fillWidth: true + value: 0.0 + maximumValue: 10.0 + minimumValue: 0.0 + decimals: 2 + stepSize: 0.10 + onEditingFinished:{ + Teleop.OnLinearVelSelection(value) + } + } + + // Angular velocity input + Label { + id: angularVelLabel + text: "Angular" + color: "dimgrey" + anchors.top: velocityLabel.bottom + anchors.topMargin: 15 + anchors.left: linearVelField.right + anchors.leftMargin: 10 + } + IgnSpinBox { + id: angularVelField + anchors.top: velocityLabel.bottom + anchors.topMargin: 5 + anchors.left: angularVelLabel.right + anchors.leftMargin: 5 + Layout.fillWidth: true + value: 0.0 + maximumValue: 2.0 + minimumValue: 0.0 + decimals: 2 + stepSize: 0.10 + onEditingFinished:{ + Teleop.OnAngularVelSelection(value) + } + } + + // Button grid + GridLayout { + id: buttonsGrid + anchors.top: angularVelField.bottom + anchors.topMargin: 15 + anchors.left: parent.left + anchors.leftMargin: 40 + Layout.fillWidth: true + columns: 4 + Button { + id: forwardButton + text: "\u25B2" + checkable: true + Layout.row: 0 + Layout.column: 1 + onClicked: { + Teleop.linearDir = forwardButton.checked ? 1 : 0 + if(backwardButton.checked) + backwardButton.checked = false + slidersSwitch.checked = false + Teleop.OnTeleopTwist() + } + ToolTip.visible: hovered + ToolTip.text: "Forward" + Material.background: Material.primary + contentItem: Label { + renderType: Text.NativeRendering + verticalAlignment: Text.AlignVCenter + horizontalAlignment: Text.AlignHCenter + font.family: "Helvetica" + font.pointSize: 10 + color: "black" + text: forwardButton.text + } + } + Button { + id: leftButton + text: "\u25C0" + checkable: true + Layout.row: 1 + Layout.column: 0 + onClicked: { + Teleop.angularDir = leftButton.checked ? 1 : 0 + if(rightButton.checked) + rightButton.checked = false + slidersSwitch.checked = false + Teleop.OnTeleopTwist() + } + Material.background: Material.primary + contentItem: Label { + renderType: Text.NativeRendering + verticalAlignment: Text.AlignVCenter + horizontalAlignment: Text.AlignHCenter + font.family: "Helvetica" + font.pointSize: 10 + color: "black" + text: leftButton.text + } + } + Button { + id: rightButton + text: "\u25B6" + checkable: true + Layout.row: 1 + Layout.column: 2 + onClicked: { + Teleop.angularDir = rightButton.checked ? -1 : 0 + if(leftButton.checked) + leftButton.checked = false + slidersSwitch.checked = false + Teleop.OnTeleopTwist() + } + Material.background: Material.primary + contentItem: Label { + renderType: Text.NativeRendering + verticalAlignment: Text.AlignVCenter + horizontalAlignment: Text.AlignHCenter + font.family: "Helvetica" + font.pointSize: 10 + color: "black" + text: rightButton.text + } + } + Button { + id: backwardButton + text: "\u25BC" + checkable: true + Layout.row: 2 + Layout.column: 1 + onClicked: { + Teleop.linearDir = backwardButton.checked ? -1 : 0 + if(forwardButton.checked) + forwardButton.checked = false + slidersSwitch.checked = false + Teleop.OnTeleopTwist() + } + Material.background: Material.primary + contentItem: Label { + renderType: Text.NativeRendering + verticalAlignment: Text.AlignVCenter + horizontalAlignment: Text.AlignHCenter + font.family: "Helvetica" + font.pointSize: 10 + color: "black" + text: backwardButton.text + } + } + Button { + id: stopButton + text: "Stop" + checkable: false + Layout.row: 1 + Layout.column: 1 + onClicked: { + Teleop.linearDir = 0 + Teleop.angularDir = 0 + forwardButton.checked = false + leftButton.checked = false + rightButton.checked = false + backwardButton.checked = false + linearVelSlider.value = 0 + angularVelSlider.value = 0 + slidersSwitch.checked = false + Teleop.OnTeleopTwist() + } + Material.background: Material.primary + contentItem: Label { + renderType: Text.NativeRendering + verticalAlignment: Text.AlignVCenter + horizontalAlignment: Text.AlignHCenter + font.family: "Helvetica" + font.pointSize: 10 + color: "black" + text: stopButton.text + } + } + } + + //Keyboard's switch + Switch { + id: keySwitch + anchors.top: buttonsGrid.bottom + anchors.topMargin: 10 + anchors.left: parent.left + anchors.leftMargin: 5 + onClicked: { + forwardButton.checked = false + leftButton.checked = false + rightButton.checked = false + backwardButton.checked = false + Teleop.OnKeySwitch(checked); + } + ToolTip.visible: hovered + ToolTip.text: checked ? qsTr("Disable keyboard") : qsTr("Enable keyboard") + } + Label { + id: keyboardSwitchLabel + text: "Input from keyboard (WASD)" + anchors.horizontalCenter : keySwitch.horizontalCenter + anchors.verticalCenter : keySwitch.verticalCenter + anchors.left: keySwitch.right + anchors.leftMargin: 5 + } + + // Slider's switch + Switch { + id: slidersSwitch + anchors.top: keySwitch.bottom + anchors.topMargin: 10 + anchors.left: keySwitch.left + onClicked: { + Teleop.OnSlidersSwitch(checked); + if(checked){ + forwardButton.checked = false + leftButton.checked = false + rightButton.checked = false + backwardButton.checked = false + linearVelField.value = linearVelSlider.value.toFixed(2) + angularVelField.value = angularVelSlider.value.toFixed(2) + Teleop.OnLinearVelSelection(linearVelSlider.value) + Teleop.OnAngularVelSelection(angularVelSlider.value) + Teleop.OnTeleopTwist() + } + } + ToolTip.visible: hovered + ToolTip.text: checked ? qsTr("Disable sliders") : qsTr("Enable sliders") + } + Label { + id: slidersSwitchLabel + text: "Input from sliders" + anchors.horizontalCenter : slidersSwitch.horizontalCenter + anchors.verticalCenter : slidersSwitch.verticalCenter + anchors.left: slidersSwitch.right + anchors.leftMargin: 5 + } + + TextField { + id: linearVelMaxTextField + anchors.top: slidersSwitch.bottom + anchors.topMargin: 10 + anchors.horizontalCenter : angularVelSlider.horizontalCenter + width: 40 + text:"1.0" + } + + // Vertical slider + Slider { + id: linearVelSlider + height: 150 + width: 50 + orientation: Qt.Vertical + anchors.top: linearVelMaxTextField.bottom + anchors.horizontalCenter : angularVelSlider.horizontalCenter + handle: Rectangle { + y: linearVelSlider.topPadding + linearVelSlider.visualPosition * (linearVelSlider.availableHeight - height) + x: linearVelSlider.leftPadding + linearVelSlider.availableWidth / 2 - width / 2 + implicitWidth: 25 + implicitHeight: 10 + color: linearVelSlider.pressed ? "#f0f0f0" : "#f6f6f6" + border.color: "black" + } + enabled: slidersSwitch.checked + + from: linearVelMinTextField.text + to: linearVelMaxTextField.text + stepSize: 0.01 + + onMoved: { + linearVelField.value = linearVelSlider.value.toFixed(2) + Teleop.OnLinearVelSelection(linearVelSlider.value) + Teleop.OnTeleopTwist() + } + } + + TextField { + id: linearVelMinTextField + anchors.top: linearVelSlider.bottom + anchors.horizontalCenter : linearVelSlider.horizontalCenter + width: 40 + text:"-1.0" + } + + Label { + id: currentLinearVelSliderLabel + anchors.verticalCenter : linearVelSlider.verticalCenter + anchors.left : linearVelSlider.right + text: linearVelSlider.value.toFixed(2) + " m/s" + } + + TextField { + id: angularVelMinTextField + anchors.verticalCenter : angularVelSlider.verticalCenter + anchors.left : slidersSwitch.left + width: 40 + text:"-1.0" + } + + // Horizontal slider + Slider { + id: angularVelSlider + height: 50 + width: 175 + anchors.top: linearVelSlider.bottom + anchors.topMargin: 50 + anchors.left : angularVelMinTextField.right + anchors.leftMargin: 10 + handle: Rectangle { + x: angularVelSlider.leftPadding + angularVelSlider.visualPosition * (angularVelSlider.availableWidth - width) + y: angularVelSlider.topPadding + angularVelSlider.availableHeight / 2 - height / 2 + implicitWidth: 10 + implicitHeight: 25 + color: angularVelSlider.pressed ? "#f0f0f0" : "#f6f6f6" + border.color: "black" + } + enabled: slidersSwitch.checked + + from: angularVelMinTextField.text + to: angularVelMaxTextField.text + stepSize: 0.01 + + onMoved: { + angularVelField.value = angularVelSlider.value.toFixed(2) + Teleop.OnAngularVelSelection(angularVelSlider.value) + Teleop.OnTeleopTwist() + } + } + + TextField { + id: angularVelMaxTextField + anchors.verticalCenter : angularVelSlider.verticalCenter + anchors.left : angularVelSlider.right + anchors.leftMargin: 10 + width: 40 + text:"1.0" + } + + Label { + id: currentAngularVelSliderLabel + anchors.horizontalCenter : angularVelSlider.horizontalCenter + anchors.top : angularVelSlider.bottom + text: angularVelSlider.value.toFixed(2) + " rad/s" + } +} diff --git a/src/plugins/teleop/Teleop.qrc b/src/plugins/teleop/Teleop.qrc new file mode 100644 index 000000000..872ced8b3 --- /dev/null +++ b/src/plugins/teleop/Teleop.qrc @@ -0,0 +1,5 @@ + + + Teleop.qml + + diff --git a/src/plugins/teleop/Teleop_TEST.cc b/src/plugins/teleop/Teleop_TEST.cc new file mode 100644 index 000000000..90c9a7c2f --- /dev/null +++ b/src/plugins/teleop/Teleop_TEST.cc @@ -0,0 +1,355 @@ +/* + * Copyright (C) 2021 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 +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include + +#include "test_config.h" // NOLINT(build/include) +#include "ignition/gui/Application.hh" +#include "ignition/gui/Plugin.hh" +#include "ignition/gui/MainWindow.hh" +#include "ignition/gui/qt.h" + +#include "Teleop.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("./Teleop_TEST")), +}; + +using namespace ignition; +using namespace gui; + +class TeleopTest : public ::testing::Test +{ + // Set up function. + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + + this->app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + + // Load plugin + const char *pluginStr = + "" + "" + "Teleop!" + "" + ""; + + tinyxml2::XMLDocument pluginDoc; + EXPECT_EQ(tinyxml2::XML_SUCCESS, pluginDoc.Parse(pluginStr)); + EXPECT_TRUE(this->app.LoadPlugin("Teleop", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + win = this->app.findChild(); + ASSERT_NE(nullptr, win); + + // Show, but don't exec, so we don't block + win->QuickWindow()->show(); + + // Get plugin + plugins = win->findChildren(); + EXPECT_EQ(plugins.size(), 1); + + plugin = plugins[0]; + EXPECT_EQ(plugin->Title(), "Teleop!"); + + // Subscribes to the command velocity topic. + node.Subscribe("/model/vehicle_blue/cmd_vel", + &TeleopTest::VerifyTwistMsgCb, this); + + // Sets topic. This must be the same as the + // one the node is subscribed to. + plugin->OnTopicSelection( + QString::fromStdString("/model/vehicle_blue/cmd_vel")); + + // Checks if the directions of the movement are set + // with the default value '0'. + EXPECT_EQ(plugin->LinearDirection(), 0); + EXPECT_EQ(plugin->AngularDirection(), 0); + + // Set velocity value and movement direction. + plugin->OnLinearVelSelection(linearVel); + plugin->OnAngularVelSelection(angularVel); + } + + // Subscriber call back function. Verifies if the Twist message is + // sent correctly. + protected: void VerifyTwistMsgCb(const msgs::Twist &_msg) + { + EXPECT_DOUBLE_EQ(_msg.linear().x(), + plugin->LinearDirection() * linearVel); + EXPECT_DOUBLE_EQ(_msg.angular().z(), + plugin->AngularDirection() * angularVel); + received = true; + } + + // Provides an API to load plugins and configuration files. + protected: Application app{g_argc, g_argv}; + + // List of plugins. + protected: QList plugins; + protected: plugins::Teleop * plugin; + + // Instance of the main window. + protected: MainWindow * win; + + // Checks if a new command has been received. + protected: bool received = false; + protected: transport::Node node; + + // Define velocity values. + protected: const double linearVel = 1.0; + protected: const double angularVel = 0.5; +}; + +///////////////////////////////////////////////// +TEST_F(TeleopTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(ButtonCommand)) +{ + // Forward movement. + plugin->setLinearDirection(1); + // Counterclockwise movement. + plugin->setAngularDirection(1); + plugin->OnTeleopTwist(); + + int sleep = 0; + const int maxSleep = 30; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + + // Change movement direction. + // Backward movement. + plugin->setLinearDirection(-1); + // Clockwise direction. + plugin->setAngularDirection(-1); + plugin->OnTeleopTwist(); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + + // Stops angular movement. + plugin->setAngularDirection(0); + plugin->OnTeleopTwist(); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + EXPECT_TRUE(received); + received = false; + + // Stops linear movement. + // Starts angular movement. + plugin->setLinearDirection(0); + plugin->setAngularDirection(1); + plugin->OnTeleopTwist(); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + + // Stops movement. + plugin->setAngularDirection(0); + plugin->setLinearDirection(0); + plugin->OnTeleopTwist(); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + + // Cleanup + plugins.clear(); +} + +///////////////////////////////////////////////// +TEST_F(TeleopTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(KeyboardCommand)) +{ + // Generates a key press event on the main window. + QKeyEvent *keypress_W = new QKeyEvent(QKeyEvent::KeyPress, + Qt::Key_W, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keypress_W); + QCoreApplication::processEvents(); + + EXPECT_FALSE(received); + + // Enables key input. + plugin->OnKeySwitch(true); + app.sendEvent(win->QuickWindow(), keypress_W); + + int sleep = 0; + const int maxSleep = 30; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + + // Generates a key press event on the main window. + QKeyEvent *keypress_D = new QKeyEvent(QKeyEvent::KeyPress, + Qt::Key_D, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keypress_D); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + + // Generates a key release event on the main window. + QKeyEvent *keyrelease_D = new QKeyEvent(QKeyEvent::KeyRelease, + Qt::Key_D, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keyrelease_D); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + + // Generates a key press event on the main window. + QKeyEvent *keypress_A = new QKeyEvent(QKeyEvent::KeyPress, + Qt::Key_A, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keypress_A); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + + // Generates a key release event on the main window. + QKeyEvent *keyrelease_A = new QKeyEvent(QKeyEvent::KeyRelease, + Qt::Key_A, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keyrelease_A); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + // Generates a key release event on the main window. + QKeyEvent *keyrelease_W = new QKeyEvent(QKeyEvent::KeyRelease, + Qt::Key_W, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keyrelease_W); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + // Generates a key press event on the main window. + QKeyEvent *keypress_X = new QKeyEvent(QKeyEvent::KeyPress, + Qt::Key_X, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keypress_X); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + EXPECT_TRUE(received); + received = false; + // Generates a key release event on the main window. + QKeyEvent *keyrelease_X = new QKeyEvent(QKeyEvent::KeyRelease, + Qt::Key_X, Qt::NoModifier); + app.sendEvent(win->QuickWindow(), keyrelease_X); + + sleep = 0; + while (!received && sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + sleep++; + } + + // Cleanup + plugins.clear(); +}