From ac1089278a063cfddc9b1322d1abcbb1ce760c00 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 9 Feb 2021 09:48:45 -0800 Subject: [PATCH 1/9] Make topics configurable for joint controllers (#584) Signed-off-by: Louise Poubel --- examples/worlds/joint_position_controller.sdf | 7 ++++--- src/systems/joint_controller/JointController.cc | 4 ++++ .../joint_position_controller/JointPositionController.cc | 4 ++++ 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/examples/worlds/joint_position_controller.sdf b/examples/worlds/joint_position_controller.sdf index 6d2ad62e83..b2bd104509 100644 --- a/examples/worlds/joint_position_controller.sdf +++ b/examples/worlds/joint_position_controller.sdf @@ -4,9 +4,9 @@ Try sending joint position commands: - ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: -1.0" + ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: -1.0" - ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: 1.0" + ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: 1.0" --> @@ -45,7 +45,7 @@ 72 121 1 - + floating @@ -156,6 +156,7 @@ filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController"> j1 + rotor_cmd 1 0.1 0.01 diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 42990d4447..695ee62d6d 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -133,6 +133,10 @@ void JointController::Configure(const Entity &_entity, // Subscribe to commands std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + "/cmd_vel"}; + if (_sdf->HasElement("topic")) + { + topic = _sdf->Get("topic"); + } this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, this->dataPtr.get()); diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index c048ff0e23..691336d901 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -150,6 +150,10 @@ void JointPositionController::Configure(const Entity &_entity, std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + "/" + std::to_string(this->dataPtr->jointIndex) + "/cmd_pos"}; + if (_sdf->HasElement("topic")) + { + topic = _sdf->Get("topic"); + } this->dataPtr->node.Subscribe( topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get()); From 87b2f4ba27cc10a1a586bda8d635aa929e1070ce Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Thu, 11 Feb 2021 02:48:59 +0530 Subject: [PATCH 2/9] Add About dialog (#609) Signed-off-by: atharva-18 --- src/gui/AboutDialogHandler.cc | 67 ++++++++++++++++++++++++++++++ src/gui/AboutDialogHandler.hh | 58 ++++++++++++++++++++++++++ src/gui/CMakeLists.txt | 1 + src/gui/Gui.cc | 5 +++ src/gui/resources/GazeboDrawer.qml | 55 +++++++++++++++++++----- 5 files changed, 176 insertions(+), 10 deletions(-) create mode 100644 src/gui/AboutDialogHandler.cc create mode 100644 src/gui/AboutDialogHandler.hh diff --git a/src/gui/AboutDialogHandler.cc b/src/gui/AboutDialogHandler.cc new file mode 100644 index 0000000000..545f6fbb66 --- /dev/null +++ b/src/gui/AboutDialogHandler.cc @@ -0,0 +1,67 @@ +/* + * 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 "AboutDialogHandler.hh" + +#include +#include +#include + +using namespace ignition; +using namespace gazebo; +using namespace gazebo::gui; + +///////////////////////////////////////////////// +AboutDialogHandler::AboutDialogHandler() +{ + aboutText += std::string(IGNITION_GAZEBO_VERSION_HEADER); + aboutText += "" + "" + "" + "" + "" + "" + "" + "" + "" + "
Documentation:" + "" + "" + "https://ignitionrobotics.org/libs/gazebo" + "" + "
" + "Tutorials:" + "" + "" + "https://ignitionrobotics.org/docs/" + "" + "
"; +} + +///////////////////////////////////////////////// +QString AboutDialogHandler::getVersionInformation() +{ + return QString::fromStdString(this->aboutText); +} + +///////////////////////////////////////////////// +void AboutDialogHandler::openURL(QString _url) +{ + QDesktopServices::openUrl(QUrl(_url)); +} diff --git a/src/gui/AboutDialogHandler.hh b/src/gui/AboutDialogHandler.hh new file mode 100644 index 0000000000..b5f71b543d --- /dev/null +++ b/src/gui/AboutDialogHandler.hh @@ -0,0 +1,58 @@ +/* + * 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_GAZEBO_GUI_ABOUTDIALOGHANDLER_HH_ +#define IGNITION_GAZEBO_GUI_ABOUTDIALOGHANDLER_HH_ + +#include +#include +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Export.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace gui +{ +/// \brief Class for handling about dialog +class IGNITION_GAZEBO_VISIBLE AboutDialogHandler : public QObject +{ + Q_OBJECT + + /// \brief Constructor + public: AboutDialogHandler(); + + /// \brief Get version information + /// \return Version information in rich text format + Q_INVOKABLE QString getVersionInformation(); + + /// \brief Function called from QML when user clicks on a link + /// \param[in] _url Url to web page. + Q_INVOKABLE void openURL(QString _url); + + /// \brief Version information and links to online resources + private: std::string aboutText; +}; +} +} +} +} +#endif diff --git a/src/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index 5d53622322..46d4be7f5c 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -1,4 +1,5 @@ set (gui_sources + AboutDialogHandler.cc Gui.cc GuiFileHandler.cc GuiRunner.cc diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 00c3b7215f..04340d116d 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -28,6 +28,7 @@ #include "ignition/gazebo/gui/TmpIface.hh" #include "ignition/gazebo/gui/Gui.hh" +#include "AboutDialogHandler.hh" #include "GuiFileHandler.hh" #include "PathManager.hh" @@ -63,6 +64,9 @@ std::unique_ptr createGui( auto tmp = new ignition::gazebo::TmpIface(); tmp->setParent(app->Engine()); + auto aboutDialogHandler = new ignition::gazebo::gui::AboutDialogHandler(); + aboutDialogHandler->setParent(app->Engine()); + auto guiFileHandler = new ignition::gazebo::gui::GuiFileHandler(); guiFileHandler->setParent(app->Engine()); @@ -102,6 +106,7 @@ std::unique_ptr createGui( // Let QML files use TmpIface' functions and properties auto context = new QQmlContext(app->Engine()->rootContext()); context->setContextProperty("TmpIface", tmp); + context->setContextProperty("AboutDialogHandler", aboutDialogHandler); context->setContextProperty("GuiFileHandler", guiFileHandler); // Instantiate GazeboDrawer.qml file into a component diff --git a/src/gui/resources/GazeboDrawer.qml b/src/gui/resources/GazeboDrawer.qml index 8ca3f273a7..647883da34 100644 --- a/src/gui/resources/GazeboDrawer.qml +++ b/src/gui/resources/GazeboDrawer.qml @@ -60,6 +60,9 @@ Rectangle { case "loadWorld": loadWorldDialog.open(); break + case "aboutDialog": + aboutDialog.open(); + break // Forward others to default drawer default: parent.onAction(_action); @@ -73,46 +76,50 @@ Rectangle { // Custom action which calls custom C++ code /*ListElement { title: "New world" - action: "newWorld" + actionElement: "newWorld" type: "world" } ListElement { title: "Load world" - action: "loadWorld" + actionElement: "loadWorld" type: "world" }*/ ListElement { title: "Save world" - action: "saveWorld" + actionElement: "saveWorld" enabled: false type: "world" } ListElement { title: "Save world as..." - action: "saveWorldAs" + actionElement: "saveWorldAs" type: "world" } // Actions provided by Ignition GUI, with custom titles ListElement { title: "Load client configuration" - action: "loadConfig" + actionElement: "loadConfig" } ListElement { title: "Save client configuration" - action: "saveConfig" + actionElement: "saveConfig" } ListElement { title: "Save client configuration as" - action: "saveConfigAs" + actionElement: "saveConfigAs" } ListElement { title: "Style settings" - action: "styleSettings" + actionElement: "styleSettings" + } + ListElement { + title: "About" + actionElement: "aboutDialog" } ListElement { title: "Quit" - action: "close" + actionElement: "close" } } @@ -125,7 +132,7 @@ Rectangle { text: title highlighted: ListView.isCurrentItem onClicked: { - customDrawer.onAction(action); + customDrawer.onAction(actionElement); customDrawer.parent.closeDrawer(); } } @@ -165,6 +172,34 @@ Rectangle { } } + /** + * About dialog + */ + Dialog { + id: aboutDialog + title: "Ignition Gazebo" + + modal: true + focus: true + parent: ApplicationWindow.overlay + width: parent.width / 3 > 500 ? 500 : parent.width / 3 + x: (parent.width - width) / 2 + y: (parent.height - height) / 2 + closePolicy: Popup.CloseOnEscape + standardButtons: StandardButton.Ok + + Text { + anchors.fill: parent + id: aboutMessage + wrapMode: Text.Wrap + verticalAlignment: Text.AlignVCenter + color: Material.theme == Material.Light ? "black" : "white" + textFormat: Text.RichText + text: AboutDialogHandler.getVersionInformation() + onLinkActivated: AboutDialogHandler.openURL(link) + } + } + /** * Dialog with configurations for SDF generation */ From e647570f25f962d63af75cf669ff72731d57bd5e Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 10 Feb 2021 14:59:20 -0800 Subject: [PATCH 3/9] Add thermal sensor system for configuring thermal camera properties (#614) Signed-off-by: Ian Chen Co-authored-by: Ashton Larkin --- examples/worlds/thermal_camera.sdf | 62 +++++ .../ignition/gazebo/components/Temperature.hh | 6 + .../ignition/gazebo/rendering/RenderUtil.hh | 6 + src/rendering/RenderUtil.cc | 118 ++++++++++ src/systems/sensors/Sensors.cc | 12 + src/systems/sensors/Sensors.hh | 5 + src/systems/thermal/CMakeLists.txt | 6 + src/systems/thermal/ThermalSensor.cc | 93 ++++++++ src/systems/thermal/ThermalSensor.hh | 60 +++++ test/integration/CMakeLists.txt | 3 +- test/integration/thermal_sensor_system.cc | 219 ++++++++++++++++++ test/integration/thermal_system.cc | 76 +++++- test/worlds/thermal.sdf | 46 ++++ test/worlds/thermal_invalid.sdf | 152 ++++++++++++ 14 files changed, 862 insertions(+), 2 deletions(-) create mode 100644 src/systems/thermal/ThermalSensor.cc create mode 100644 src/systems/thermal/ThermalSensor.hh create mode 100644 test/integration/thermal_sensor_system.cc create mode 100644 test/worlds/thermal_invalid.sdf diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf index cf5c92b3f4..e21215537a 100644 --- a/examples/worlds/thermal_camera.sdf +++ b/examples/worlds/thermal_camera.sdf @@ -104,6 +104,18 @@ thermal_camera false + + + Thermal camera 8 Bit + floating + 350 + 315 + 500 + + thermal_camera_8bit/image + false + + @@ -230,6 +242,7 @@ 320 240 + L16 0.1 @@ -245,6 +258,55 @@ true + + 4.5 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_8bit/image + + 253.15 + 673.15 + 3.0 + + + + + true + + + 1 0 0 0 0 1.570796 rescue_randy diff --git a/include/ignition/gazebo/components/Temperature.hh b/include/ignition/gazebo/components/Temperature.hh index 228e208518..cfd8df5327 100644 --- a/include/ignition/gazebo/components/Temperature.hh +++ b/include/ignition/gazebo/components/Temperature.hh @@ -35,6 +35,12 @@ namespace components using Temperature = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Temperature", Temperature) + + /// \brief A component that stores temperature linear resolution in Kelvin + using TemperatureLinearResolution = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.TemperatureLinearResolution", + TemperatureLinearResolution) } } } diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 600744db97..5d64553867 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -66,6 +66,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Pointer to the scene public: rendering::ScenePtr Scene() const; + /// \brief Helper Update function for updating the scene + /// \param[in] _info Sim update info + /// \param[in] _ecm Mutable reference to the entity component manager + public: void UpdateECM(const UpdateInfo &_info, + EntityComponentManager &_ecm); + /// \brief Helper PostUpdate function for updating the scene public: void UpdateFromECM(const UpdateInfo &_info, const EntityComponentManager &_ecm); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 563078231a..c1c507713b 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -262,6 +262,12 @@ class ignition::gazebo::RenderUtilPrivate /// \brief A map of created collision entities and if they are currently /// visible public: std::map viewingCollisions; + + /// \brief A map of entity id to thermal camera sensor configuration + /// properties. The elements in the tuple are: + /// + public:std::unordered_map> thermalCameraData; }; ////////////////////////////////////////////////// @@ -278,6 +284,53 @@ rendering::ScenePtr RenderUtil::Scene() const return this->dataPtr->scene; } +////////////////////////////////////////////////// +void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + std::lock_guard lock(this->dataPtr->updateMutex); + // Update thermal cameras + _ecm.Each( + [&](const Entity &_entity, + const components::ThermalCamera *)->bool + { + // set properties from thermal sensor plugin + // Set defaults to invaid values so we know they have not been set. + // set UpdateECM(). We check for valid values first before setting + // these thermal camera properties.. + double resolution = 0.0; + components::TemperatureRangeInfo range; + range.min = std::numeric_limits::max(); + range.max = 0; + + // resolution + auto resolutionComp = + _ecm.Component(_entity); + if (resolutionComp != nullptr) + { + resolution = resolutionComp->Data(); + _ecm.RemoveComponent( + _entity); + } + + // min / max temp + auto tempRangeComp = + _ecm.Component(_entity); + if (tempRangeComp != nullptr) + { + range = tempRangeComp->Data(); + _ecm.RemoveComponent(_entity); + } + + if (resolutionComp || tempRangeComp) + { + this->dataPtr->thermalCameraData[_entity] = + std::make_tuple(resolution, range); + } + return true; + }); +} + ////////////////////////////////////////////////// void RenderUtil::UpdateFromECM(const UpdateInfo &_info, const EntityComponentManager &_ecm) @@ -367,6 +420,7 @@ void RenderUtil::Update() auto actorAnimationData = std::move(this->dataPtr->actorAnimationData); auto entityTemp = std::move(this->dataPtr->entityTemp); auto newCollisionLinks = std::move(this->dataPtr->newCollisionLinks); + auto thermalCameraData = std::move(this->dataPtr->thermalCameraData); this->dataPtr->newScenes.clear(); this->dataPtr->newModels.clear(); @@ -381,6 +435,7 @@ void RenderUtil::Update() this->dataPtr->actorAnimationData.clear(); this->dataPtr->entityTemp.clear(); this->dataPtr->newCollisionLinks.clear(); + this->dataPtr->thermalCameraData.clear(); this->dataPtr->markerManager.Update(); @@ -734,6 +789,44 @@ void RenderUtil::Update() } } } + + // update thermal camera + for (const auto &thermal : this->dataPtr->thermalCameraData) + { + Entity id = thermal.first; + rendering::ThermalCameraPtr camera = + std::dynamic_pointer_cast( + this->dataPtr->sceneManager.NodeById(id)); + if (camera) + { + double resolution = std::get<0>(thermal.second); + + if (resolution > 0.0) + { + camera->SetLinearResolution(resolution); + } + else + { + ignwarn << "Unable to set thermal camera temperature linear resolution." + << " Value must be greater than 0. Using the default value: " + << camera->LinearResolution() << ". " << std::endl; + } + double minTemp = std::get<1>(thermal.second).min.Kelvin(); + double maxTemp = std::get<1>(thermal.second).max.Kelvin(); + if (maxTemp >= minTemp) + { + camera->SetMinTemperature(minTemp); + camera->SetMaxTemperature(maxTemp); + } + else + { + ignwarn << "Unable to set thermal camera temperature range." + << "Max temperature must be greater or equal to min. " + << "Using the default values : [" << camera->MinTemperature() + << ", " << camera->MaxTemperature() << "]." << std::endl; + } + } + } } ////////////////////////////////////////////////// @@ -1062,6 +1155,31 @@ void RenderUtilPrivate::CreateRenderingEntities( visual.SetMaterial(material->Data()); } + if (auto temp = _ecm.Component(_entity)) + { + // get the uniform temperature for the entity + this->entityTemp[_entity] = + std::make_tuple( + temp->Data().Kelvin(), 0.0, ""); + } + else + { + // entity doesn't have a uniform temperature. Check if it has + // a heat signature with an associated temperature range + auto heatSignature = + _ecm.Component(_entity); + auto tempRange = + _ecm.Component(_entity); + if (heatSignature && tempRange) + { + this->entityTemp[_entity] = + std::make_tuple( + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); + } + } + this->newVisuals.push_back( std::make_tuple(_entity, visual, _parent->Data())); return true; diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 20a929854d..93d14fb354 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -412,6 +412,17 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->Run(); } +////////////////////////////////////////////////// +void Sensors::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("Sensors::Update"); + if (this->dataPtr->running && this->dataPtr->initialized) + { + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + } +} + ////////////////////////////////////////////////// void Sensors::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) @@ -592,6 +603,7 @@ std::string Sensors::CreateSensor(const Entity &_entity, IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, + Sensors::ISystemUpdate, Sensors::ISystemPostUpdate ) diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index b295159cf8..3ee5807268 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -42,6 +42,7 @@ namespace systems class IGNITION_GAZEBO_VISIBLE Sensors: public System, public ISystemConfigure, + public ISystemUpdate, public ISystemPostUpdate { /// \brief Constructor @@ -56,6 +57,10 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; diff --git a/src/systems/thermal/CMakeLists.txt b/src/systems/thermal/CMakeLists.txt index ff3e7aa265..c6d6dfc3ba 100644 --- a/src/systems/thermal/CMakeLists.txt +++ b/src/systems/thermal/CMakeLists.txt @@ -5,3 +5,9 @@ gz_add_system(thermal ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) +gz_add_system(thermal-sensor + SOURCES + ThermalSensor.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/thermal/ThermalSensor.cc b/src/systems/thermal/ThermalSensor.cc new file mode 100644 index 0000000000..72e5f60a45 --- /dev/null +++ b/src/systems/thermal/ThermalSensor.cc @@ -0,0 +1,93 @@ +/* + * 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 "ThermalSensor.hh" + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private Thermal sensor data class. +class ignition::gazebo::systems::ThermalSensorPrivate +{ +}; + +////////////////////////////////////////////////// +ThermalSensor::ThermalSensor() : System(), + dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +ThermalSensor::~ThermalSensor() = default; + +////////////////////////////////////////////////// +void ThermalSensor::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm, + gazebo::EventManager & /*_eventMgr*/) +{ + const std::string resolutionTag = "resolution"; + const std::string minTempTag = "min_temp"; + const std::string maxTempTag = "max_temp"; + + auto sensorComp = _ecm.Component(_entity); + if (sensorComp == nullptr) + { + ignerr << "The thermal sensor system can only be used to configure " + << "parameters of thermal camera sensor " << std::endl; + return; + } + + if (_sdf->HasElement(resolutionTag)) + { + double resolution = _sdf->Get(resolutionTag); + _ecm.CreateComponent(_entity, + components::TemperatureLinearResolution(resolution)); + } + + if (_sdf->HasElement(minTempTag) || _sdf->HasElement(maxTempTag)) + { + double min = 0.0; + double max = std::numeric_limits::max(); + min = _sdf->Get(minTempTag, min).first; + max = _sdf->Get(maxTempTag, max).first; + components::TemperatureRangeInfo rangeInfo; + rangeInfo.min = min; + rangeInfo.max = max; + _ecm.CreateComponent(_entity, components::TemperatureRange(rangeInfo)); + } +} + +IGNITION_ADD_PLUGIN(ThermalSensor, System, + ThermalSensor::ISystemConfigure +) + +IGNITION_ADD_PLUGIN_ALIAS(ThermalSensor, + "ignition::gazebo::systems::ThermalSensor") diff --git a/src/systems/thermal/ThermalSensor.hh b/src/systems/thermal/ThermalSensor.hh new file mode 100644 index 0000000000..ed2fb431f2 --- /dev/null +++ b/src/systems/thermal/ThermalSensor.hh @@ -0,0 +1,60 @@ +/* + * 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_GAZEBO_SYSTEMS_THERMALSENSOR_HH_ +#define IGNITION_GAZEBO_SYSTEMS_THERMALSENSOR_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ThermalSensorPrivate; + + /// \brief A thermal sensor plugin for configuring thermal sensor properties + class IGNITION_GAZEBO_VISIBLE ThermalSensor: + public System, + public ISystemConfigure + { + /// \brief Constructor + public: explicit ThermalSensor(); + + /// \brief Destructor + public: ~ThermalSensor() override; + + /// Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index a2e4eb484f..5e2233fbd0 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -42,7 +42,6 @@ set(tests scene_broadcaster_system.cc sdf_frame_semantics.cc sdf_include.cc - thermal_system.cc touch_plugin.cc triggered_publisher.cc user_commands.cc @@ -60,6 +59,8 @@ set(tests_needing_display gpu_lidar.cc rgbd_camera.cc sensors_system.cc + thermal_system.cc + thermal_sensor_system.cc ) # Add systems that need a valid display here. diff --git a/test/integration/thermal_sensor_system.cc b/test/integration/thermal_sensor_system.cc new file mode 100644 index 0000000000..472a8a60ff --- /dev/null +++ b/test/integration/thermal_sensor_system.cc @@ -0,0 +1,219 @@ +/* + * 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 + +#include +#include + +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test Thermal system +class ThermalSensorTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } +}; + + +std::mutex g_mutex; +std::vector g_imageMsgs; +unsigned char *g_image = nullptr; + +///////////////////////////////////////////////// +void thermalCb(const msgs::Image &_msg) +{ + std::lock_guard g_lock(g_mutex); + g_imageMsgs.push_back(_msg); + + unsigned int width = _msg.width(); + unsigned int height = _msg.height(); + unsigned int size = width * height * sizeof(unsigned char); + if (!g_image) + { + g_image = new unsigned char[size]; + } + memcpy(g_image, _msg.data().c_str(), size); +} + +///////////////////////////////////////////////// +TEST_F(ThermalSensorTest, + IGN_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystemInvalidConfig)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test/worlds/thermal_invalid.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that checks for thermal component + test::Relay testSystem; + + double resolution = 0; + double minTemp = std::numeric_limits::max(); + double maxTemp = 0.0; + std::map entityTemp; + std::string name; + sdf::Sensor sensorSdf; + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::Temperature *_temp, + const components::Name *_name) -> bool + { + // store temperature data + entityTemp[_name->Data()] = _temp->Data(); + + // verify temperature data belongs to a visual + EXPECT_NE(nullptr, _ecm.Component(_id)); + + return true; + }); + }); + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::ThermalCamera *_sensor, + const components::Name *_name) -> bool + { + // store temperature data + sensorSdf = _sensor->Data(); + name = _name->Data(); + + auto resolutionComp = + _ecm.Component( + _id); + if (resolutionComp) + resolution = resolutionComp->Data(); + + auto temperatureRangeComp = + _ecm.Component(_id); + if (temperatureRangeComp) + { + auto info = temperatureRangeComp->Data(); + minTemp = info.min.Kelvin(); + maxTemp = info.max.Kelvin(); + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // subscribe to thermal topic + transport::Node node; + node.Subscribe("/thermal_camera_invalid/image", &thermalCb); + + // Run server + server.Run(true, 1, false); + + // verify camera properties from sdf + unsigned int width = 320u; + unsigned int height = 240u; + EXPECT_EQ("thermal_camera_invalid", name); + const sdf::Camera *cameraSdf = sensorSdf.CameraSensor(); + ASSERT_NE(nullptr, cameraSdf); + EXPECT_EQ(width, cameraSdf->ImageWidth()); + EXPECT_EQ(height, cameraSdf->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat()); + + // verify camera properties set through plugin + // the resolution, min and max are invalid range values. Ign-gazebo should + // print out warnings and use default values + EXPECT_DOUBLE_EQ(0.0, resolution); + EXPECT_DOUBLE_EQ(999.0, minTemp); + EXPECT_DOUBLE_EQ(-590.0, maxTemp); + + // verify temperature of heat source + const std::string sphereVisual = "sphere_visual"; + const std::string cylinderVisual = "cylinder_visual"; + EXPECT_EQ(2u, entityTemp.size()); + ASSERT_TRUE(entityTemp.find(sphereVisual) != entityTemp.end()); + ASSERT_TRUE(entityTemp.find(cylinderVisual) != entityTemp.end()); + EXPECT_DOUBLE_EQ(600.0, entityTemp[sphereVisual].Kelvin()); + // the user specified temp is larger than the max value representable by an + // 8 bit 3 degree resolution camera - this value should be clamped + EXPECT_DOUBLE_EQ(800.0, entityTemp[cylinderVisual].Kelvin()); + + // Run server + server.Run(true, 10, false); + + // wait for image + bool received = false; + for (unsigned int i = 0; i < 20; ++i) + { + { + std::lock_guard lock(g_mutex); + received = !g_imageMsgs.empty(); + } + if (received) + break; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(received); + + // check temperature in actual image output + { + std::lock_guard lock(g_mutex); + unsigned int leftIdx = height * 0.5 * width; + unsigned int rightIdx = leftIdx + width-1; + unsigned int defaultResolution = 3u; + unsigned int cylinderTemp = g_image[leftIdx] * defaultResolution; + unsigned int sphereTemp = g_image[rightIdx] * defaultResolution; + // default resolution, min, max values used so we should get correct + // temperature value + EXPECT_EQ(600u, sphereTemp); + // 8 bit 3 degree resolution camera - this value should be clamped + // in the image output to: + // 2^(bitDepth) - 1 * resolution = 2^8 - 1 * 3 = 765 + EXPECT_EQ(765u, cylinderTemp); + } + + g_imageMsgs.clear(); + delete [] g_image; + g_image = nullptr; +} diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index 211f06b28d..25b8a98bb0 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -16,14 +16,21 @@ */ #include + +#include +#include + #include +#include #include #include +#include #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" #include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" @@ -47,7 +54,7 @@ class ThermalTest : public ::testing::Test }; ///////////////////////////////////////////////// -TEST_F(ThermalTest, TemperatureComponent) +TEST_F(ThermalTest, IGN_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent)) { // Start server ServerConfig serverConfig; @@ -169,3 +176,70 @@ TEST_F(ThermalTest, TemperatureComponent) EXPECT_TRUE(heatSignatures[heatSignatureSphereVisual2].find( heatSignatureTestResource) != std::string::npos); } + +///////////////////////////////////////////////// +TEST_F(ThermalTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystem)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test/worlds/thermal.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that checks for thermal component + test::Relay testSystem; + + double resolution = 0; + double minTemp = std::numeric_limits::max(); + double maxTemp = 0.0; + std::string name; + sdf::Sensor sensorSdf; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::ThermalCamera *_sensor, + const components::Name *_name) -> bool + { + // store temperature data + sensorSdf = _sensor->Data(); + name = _name->Data(); + + auto resolutionComp = + _ecm.Component( + _id); + EXPECT_NE(nullptr, resolutionComp); + resolution = resolutionComp->Data(); + + auto temperatureRangeComp = + _ecm.Component(_id); + EXPECT_NE(nullptr, temperatureRangeComp); + auto info = temperatureRangeComp->Data(); + minTemp = info.min.Kelvin(); + maxTemp = info.max.Kelvin(); + return true; + }); + + }); + server.AddSystem(testSystem.systemPtr); + + // Run server + server.Run(true, 1, false); + + // verify camera properties from sdf + EXPECT_EQ("thermal_camera_8bit", name); + const sdf::Camera *cameraSdf = sensorSdf.CameraSensor(); + ASSERT_NE(nullptr, cameraSdf); + EXPECT_EQ(320u, cameraSdf->ImageWidth()); + EXPECT_EQ(240u, cameraSdf->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat()); + + // verify camera properties set through plugin + EXPECT_DOUBLE_EQ(3.0, resolution); + EXPECT_DOUBLE_EQ(253.15, minTemp); + EXPECT_DOUBLE_EQ(673.15, maxTemp); +} diff --git a/test/worlds/thermal.sdf b/test/worlds/thermal.sdf index 73cc58c228..5274b749b0 100644 --- a/test/worlds/thermal.sdf +++ b/test/worlds/thermal.sdf @@ -309,5 +309,51 @@ https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy + + 4.5 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_8bit/image + + 253.15 + 673.15 + 3.0 + + + + true + diff --git a/test/worlds/thermal_invalid.sdf b/test/worlds/thermal_invalid.sdf new file mode 100644 index 0000000000..89c621b9c9 --- /dev/null +++ b/test/worlds/thermal_invalid.sdf @@ -0,0 +1,152 @@ + + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + 310 + + + + true` + 0 0.5 0.5 0 0 0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + 600.0 + + + + + + + true` + 0 -0.5 0.5 0 0 0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + 800.0 + + + + + + + 1.0 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_invalid/image + + 999 + -590 + 0.0 + + + + true + + + From d959c41136713b7f9dc48aee92ac6a8726116976 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Wed, 10 Feb 2021 21:42:23 -0500 Subject: [PATCH 4/9] 4.4.0 (#625) * 4.4.0 Signed-off-by: Ashton Larkin * fix typo Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d9ceb3e6fa..f11b8106de 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-gazebo4 VERSION 4.3.0) +project(ignition-gazebo4 VERSION 4.4.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 8a76252c42..5e7fc77a9b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,22 @@ ## Ignition Gazebo 4.x +### Ignition Gazebo 4.4.0 (2020-02-10) + +1. Added issue and PR templates + * [Pull Request 613](https://github.com/ignitionrobotics/ign-gazebo/pull/613) + +1. Fix segfault in SetRemovedComponentsMsgs method + * [Pull Request 495](https://github.com/ignitionrobotics/ign-gazebo/pull/495) + +1. Make topics configurable for joint controllers + * [Pull Request 584](https://github.com/ignitionrobotics/ign-gazebo/pull/584) + +1. Add about dialog + * [Pull Request 609](https://github.com/ignitionrobotics/ign-gazebo/pull/609) + +1. Add thermal sensor system for configuring thermal camera properties + * [Pull Request 614](https://github.com/ignitionrobotics/ign-gazebo/pull/614) + ### Ignition Gazebo 4.3.0 (2020-02-02) 1. Non-blocking paths request. From 14cc8370f052bf5139018d472914491cf6fef3a4 Mon Sep 17 00:00:00 2001 From: Andrej Orsula Date: Thu, 11 Feb 2021 19:50:34 +0100 Subject: [PATCH 5/9] Add JointTrajectoryController system plugin (#473) Signed-off-by: Andrej Orsula Co-authored-by: Louise Poubel --- CMakeLists.txt | 2 +- .../worlds/joint_trajectory_controller.sdf | 722 +++++++++++ src/systems/CMakeLists.txt | 1 + .../CMakeLists.txt | 7 + .../JointTrajectoryController.cc | 1067 +++++++++++++++++ .../JointTrajectoryController.hh | 164 +++ test/integration/CMakeLists.txt | 1 + .../joint_trajectory_controller_system.cc | 399 ++++++ test/worlds/joint_trajectory_controller.sdf | 420 +++++++ 9 files changed, 2782 insertions(+), 1 deletion(-) create mode 100644 examples/worlds/joint_trajectory_controller.sdf create mode 100644 src/systems/joint_trajectory_controller/CMakeLists.txt create mode 100644 src/systems/joint_trajectory_controller/JointTrajectoryController.cc create mode 100644 src/systems/joint_trajectory_controller/JointTrajectoryController.hh create mode 100644 test/integration/joint_trajectory_controller_system.cc create mode 100644 test/worlds/joint_trajectory_controller.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index f11b8106de..b616eadfec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,7 +53,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport9_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs6 REQUIRED) +ign_find_package(ignition-msgs6 REQUIRED VERSION 6.2) set(IGN_MSGS_VER ${ignition-msgs6_VERSION_MAJOR}) #-------------------------------------- diff --git a/examples/worlds/joint_trajectory_controller.sdf b/examples/worlds/joint_trajectory_controller.sdf new file mode 100644 index 0000000000..29b68f69a5 --- /dev/null +++ b/examples/worlds/joint_trajectory_controller.sdf @@ -0,0 +1,722 @@ + + + + + + + + + + + + + 0.4 0.4 0.4 + false + + + + + + + + + + 3D View + false + docked + + ogre + scene + 1 0 0 0 0 3.1416 + + + + + + World control + false + false + 50 + 100 + 1 + floating + + + + + + true + true + true + + + + + + World stats + false + false + 250 + 110 + 1 + floating + + + + + + true + true + true + true + + + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + -0.1 0 0 0 1.5708 0 + true + + + + + 0 0 1 + 5 5 + + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + + + + + 0 0 0.25 -2.3561945 0 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + RR_position_control_joint1 + 0.7854 + 20 + 0.4 + 1.0 + -1 + 1 + -20 + 20 + + RR_position_control_joint2 + -1.5708 + 10 + 0.2 + 0.5 + -1 + 1 + -10 + 10 + + + + + + + 0 -0.5 -0.25 0 0 0 + + + world + RR_velocity_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_velocity_control_link0 + RR_velocity_control_link1 + + 1 0 0 + + + 0.02 + + + + 0 0 0.1 0 0 0 + RR_velocity_control_link1 + RR_velocity_control_link2 + + 1 0 0 + + + 0.01 + + + + + + 0.6 + 175 + -10 + 10 + + 0.1 + 200 + -5 + 5 + + + + + + + 0 0.5 -0.25 -0.7854 0 0 + + + world + RR_effort_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0 0.5 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0 0.5 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_effort_control_link0 + RR_effort_control_link1 + + 1 0 0 + + 0.02 + + + + + 0 0 0.1 0 0 0 + RR_effort_control_link1 + RR_effort_control_link2 + + 1 0 0 + + 0.01 + + + + + + + custom_topic_effort_control + + + + + diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index d8b61a4056..07b88dfba4 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -86,6 +86,7 @@ add_subdirectory(imu) add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) +add_subdirectory(joint_trajectory_controller) add_subdirectory(kinetic_energy_monitor) add_subdirectory(lift_drag) add_subdirectory(log) diff --git a/src/systems/joint_trajectory_controller/CMakeLists.txt b/src/systems/joint_trajectory_controller/CMakeLists.txt new file mode 100644 index 0000000000..c4150b372e --- /dev/null +++ b/src/systems/joint_trajectory_controller/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(joint-trajectory-controller + SOURCES + JointTrajectoryController.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/joint_trajectory_controller/JointTrajectoryController.cc b/src/systems/joint_trajectory_controller/JointTrajectoryController.cc new file mode 100644 index 0000000000..d7cb345e60 --- /dev/null +++ b/src/systems/joint_trajectory_controller/JointTrajectoryController.cc @@ -0,0 +1,1067 @@ +/* + * 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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "JointTrajectoryController.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Helper class that contains all parameters required to create and +/// configure an instance of ActuatedJoint +class JointParameters +{ + /// \brief Parse all parameters required for creation of ActuatedJoint and + /// return them in a map + /// \param[in] _sdf SDF reference used to obtain the parameters + /// \param[in] _ecm Ignition Entity Component Manager + /// \param[in] _enabledJoints List of joint entities that are enabled and + /// need to be created + /// \return Map of parameters for each joint, the first entry of pair + /// indicates the joint name + public: static std::map ParseAll( + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + std::vector _enabledJoints); + + /// \brief Parse all values of a single parameter that is specified multiple + /// times in SDF + /// \param[in] _sdf SDF reference used to obtain the parameters + /// \param[in] _parameterName Name of the repeated parameter to parse all + /// values for + /// \return Ordered list of all values for a given repeated parameter + public: template static std::vector Parse( + const std::shared_ptr &_sdf, + const std::string &_parameterName); + + /// \brief Return value from `_vec` at `_index`, or `_alternative_value` if + /// `_vec` is not large enough + /// \param[in] _vec Vector that contains desired value of type T + /// \param[in] _index Index at which the value is stored + /// \param[in] _alternative_value Alternative or default value of type T + /// \return Value from `_vec` at `_index`, or `_alternative_value` if `_vec` + /// is not large enough + public: template static T NthElementOr( + const std::vector &_vec, + const size_t &_index, + const T &_alternative_value); + + /// \brief Initial position of the joint + public: double initialPosition; + /// \brief Default value for initial position of the joint + public: static constexpr double initialPositionDefault = 0.0; + + /// \brief Parameters required for creation of new PID controller + public: struct PID + { + /// \brief Proportional gain + double pGain; + /// \brief Default value for proportional gain + static constexpr double pGainDefault = 0.0; + + /// \brief Integral gain + double iGain; + /// \brief Default value for integral gain + static constexpr double iGainDefault = 0.0; + + /// \brief Derivative gain + double dGain; + /// \brief Default value for derivative gain + static constexpr double dGainDefault = 0.0; + + /// \brief Integral lower limit + double iMin; + /// \brief Default value for integral lower limit + static constexpr double iMinDefault = 0.0; + + /// \brief Integral upper limit + double iMax; + /// \brief Default value for integral upper limit + static constexpr double iMaxDefault = -1.0; + + /// \brief Output min value + double cmdMin; + /// \brief Default value for output min value + static constexpr double cmdMinDefault = 0.0; + + /// \brief Output max value + double cmdMax; + /// \brief Default value for output max value + static constexpr double cmdMaxDefault = -1.0; + + /// \brief Output offset + double cmdOffset; + /// \brief Default value for output offset + static constexpr double cmdOffsetDefault = 0.0; + } positionPID, velocityPID; +}; + +/// \brief A single 1-axis joint that is controlled by JointTrajectoryController +/// plugin +class ActuatedJoint +{ + /// \brief Default contructor + public: ActuatedJoint() = default; + + /// \brief Constructor that properly configures the actuated joint + /// \param[in] _entity Entity of the joint + /// \param[in] _params All parameters of the joint required for its + /// configuration + public: ActuatedJoint(const Entity &_entity, + const JointParameters &_params); + + /// \brief Setup components required for control of this joint + /// \param[in,out] _ecm Ignition Entity Component Manager + public: void SetupComponents( + ignition::gazebo::EntityComponentManager &_ecm) const; + + /// \brief Set target of the joint that the controller will attempt to reach + /// \param[in] _targetPoint Targets of all controlled joint + /// \param[in] _jointIndex Index of the joint, used to determine what index + /// of `_targetPoint` to use + public: void SetTarget( + const ignition::msgs::JointTrajectoryPoint &_targetPoint, + const size_t &_jointIndex); + + /// \brief Update command force that is applied on the joint + /// \param[in,out] _ecm Ignition Entity Component Manager + /// \param[in] _dt Time difference to update for + public: void Update(ignition::gazebo::EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_dt); + + /// \brief Reset the target of the joint + public: void ResetTarget(); + + /// \brief Reset the position and velocity PID error on the joint + public: void ResetPIDs(); + + /// \brief Entity of the joint + public: Entity entity; + + /// \brief Target state that the joint controller should reach + public: struct TargetState + { + /// \brief Target position of the joint + double position; + /// \brief Target position of the joint + double velocity; + /// \brief Target acceleration of the joint + /// \attention Acceleration control is NOT implemented at the moment + double acceleration; + /// \brief Target force or torque of the joint + double effort; + } target; + + /// \brief Initial position of the joint + public: double initialPosition; + + /// \brief List of PID controllers used for the control of this actuated joint + public: struct PIDs + { + /// \brief Position PID controller + ignition::math::PID position; + /// \brief Velocity PID controller + ignition::math::PID velocity; + } pids; +}; + +/// \brief Information about trajectory that is followed by +/// JointTrajectoryController plugin +class Trajectory +{ + /// \brief Update index of trajectory points, such that it directs to a point + /// that needs to be currently followed + /// \param[in] _simTime Current simulation time + /// \return True if index of the trajectory point was updated, False otherwise + public: bool UpdateCurrentPoint( + const std::chrono::steady_clock::duration &_simTime); + + /// \brief Determine if the trajectory goal was reached + /// \return True if trajectory goal was reached, False otherwise + public: bool IsGoalReached() const; + + /// \brief Compute progress of the current trajectory + /// \return Fraction of the completed points in range (0.0, 1.0] + public: float ComputeProgress() const; + + /// \brief Reset trajectory internals, i.e. clean list of joint names, points + /// and reset index of the current point + public: void Reset(); + + /// \brief Status of the trajectory + public: enum TrajectoryStatus + { + /// \brief Trajectory is new and needs to be configure on the next update + /// loop + New, + /// \brief Trajectory is currently being followed + Active, + /// \brief Trajectory goal is reached + Reached, + } status; + + /// \brief Start time of trajectory + public: std::chrono::steady_clock::duration startTime; + + /// \brief Index of the current trajectory point + public: unsigned int pointIndex; + + /// \brief Ordered joints that need to be actuated to follow the current + /// trajectory + public: std::vector jointNames; + + /// \brief Trajectory defined in terms of temporal points, whose members are + /// ordered according to `jointNames` + public: std::vector points; +}; + +/// \brief Private data of the JointTrajectoryController plugin +class ignition::gazebo::systems::JointTrajectoryControllerPrivate +{ + /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no + /// joint names are specified in the plugin configuration, all valid 1-axis + /// joints are returned + /// \param[in] _entity Entity of the model that the plugin is being + /// configured for + /// \param[in] _sdf SDF reference used to determine enabled joints + /// \param[in] _ecm Ignition Entity Component Manager + /// \return List of entities containinig all enabled joints + public: std::vector GetEnabledJoints( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm) const; + + /// \brief Callback for joint trajectory subscription + /// \param[in] _msg A new message describing a joint trajectory that needs + /// to be followed + public: void JointTrajectoryCallback( + const ignition::msgs::JointTrajectory &_msg); + + /// \brief Reset internals of the plugin, without affecting already created + /// components + public: void Reset(); + + /// \brief Ignition communication node + public: transport::Node node; + + /// \brief Publisher of the progress for currently followed trajectory + public: transport::Node::Publisher progressPub; + + /// \brief Map of actuated joints, where the first entry of pair is the name + /// of the joint + public: std::map actuatedJoints; + + /// \brief Mutex projecting trajectory + public: std::mutex trajectoryMutex; + + /// \brief Information about trajectory that should be followed + public: Trajectory trajectory; + + /// \brief Flag that determines whether to use message header timestamp as + /// the trajectory start, where simulation time at the beginning of execution + /// is used otherwise + public: bool useHeaderStartTime; + + /// \brief Flag that determines if all components required for control are + /// already setup + public: bool componentSetupFinished; +}; + +///////////////////////////////// +/// JointTrajectoryController /// +///////////////////////////////// + +////////////////////////////////////////////////// +JointTrajectoryController::JointTrajectoryController() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void JointTrajectoryController::Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager & /*_eventManager*/) +{ + // Make sure the controller is attached to a valid model + const auto model = Model(_entity); + if (!model.Valid(_ecm)) + { + ignerr << "[JointTrajectoryController] Failed to initialize because [" + << model.Name(_ecm) << "(Entity=" << _entity + << ")] is not a model. Please make sure that" + " JointTrajectoryController is attached to a valid model.\n"; + return; + } + ignmsg << "[JointTrajectoryController] Setting up controller for [" + << model.Name(_ecm) << "(Entity=" << _entity << ")].\n"; + + // Get list of enabled joints + const auto enabledJoints = this->dataPtr->GetEnabledJoints(_entity, + _sdf, + _ecm); + + // For each enabled joint, parse all of its parameters from SDF + auto jointParameters = JointParameters::ParseAll(_sdf, _ecm, enabledJoints); + + // Iterate over all enabled joints and create/configure them + for (const auto &jointEntity : enabledJoints) + { + const auto jointName = + _ecm.Component(jointEntity)->Data(); + this->dataPtr->actuatedJoints[jointName] = + ActuatedJoint(jointEntity, jointParameters[jointName]); + ignmsg << "[JointTrajectoryController] Configured joint [" + << jointName << "(Entity=" << jointEntity << ")].\n"; + } + + // Make sure at least one joint is configured + if (this->dataPtr->actuatedJoints.empty()) + { + ignerr << "[JointTrajectoryController] Failed to initialize because [" + << model.Name(_ecm) << "(Entity=" << _entity + << ")] has no supported joints.\n"; + return; + } + + // Get additional parameters from SDF + if (_sdf->HasAttribute("use_header_start_time")) + { + this->dataPtr->useHeaderStartTime = + _sdf->Get("use_header_start_time"); + } + else + { + this->dataPtr->useHeaderStartTime = false; + } + + // Subscribe to joint trajectory commands + auto trajectoryTopic = _sdf->Get("topic"); + if (trajectoryTopic.empty()) + { + // If not specified, use the default topic based on model name + trajectoryTopic = "/model/" + model.Name(_ecm) + "/joint_trajectory"; + } + // Make sure the topic is valid + const auto validTrajectoryTopic = transport::TopicUtils::AsValidTopic( + trajectoryTopic); + if (validTrajectoryTopic.empty()) + { + ignerr << "[JointTrajectoryController] Cannot subscribe to invalid topic [" + << trajectoryTopic << "].\n"; + return; + } + // Subscribe + ignmsg << "[JointTrajectoryController] Subscribing to joint trajectory" + " commands on topic [" << validTrajectoryTopic << "].\n"; + this->dataPtr->node.Subscribe( + validTrajectoryTopic, + &JointTrajectoryControllerPrivate::JointTrajectoryCallback, + this->dataPtr.get()); + + // Advertise progress + const auto progressTopic = validTrajectoryTopic + "_progress"; + ignmsg << "[JointTrajectoryController] Advertising joint trajectory progress" + " on topic [" << progressTopic << "].\n"; + this->dataPtr->progressPub = + this->dataPtr->node.Advertise(progressTopic); +} + +////////////////////////////////////////////////// +void JointTrajectoryController::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("JointTrajectoryController::PreUpdate"); + + // Create required components for each joint (only once) + if (!this->dataPtr->componentSetupFinished) + { + for (auto &actuatedJoint : this->dataPtr->actuatedJoints) + { + ActuatedJoint *joint = &actuatedJoint.second; + joint->SetupComponents(_ecm); + } + this->dataPtr->componentSetupFinished = true; + } + + // Reset plugin if jump back in time is detected + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignmsg << "[JointTrajectoryController] Resetting plugin because jump back" + " in time [" + << std::chrono::duration_cast(_info.dt).count() + << " s] was detected.\n"; + this->dataPtr->Reset(); + } + + // Nothing else to do if paused + if (_info.paused) + { + return; + } + + // Update joint targets based on the current trajectory + { + auto isTargetUpdateRequired = false; + + // Lock mutex before accessing trajectory + std::lock_guard lock(this->dataPtr->trajectoryMutex); + + if (this->dataPtr->trajectory.status == Trajectory::New) + { + // Set trajectory start time if not set before + if (this->dataPtr->trajectory.startTime.count() == 0) + { + this->dataPtr->trajectory.startTime = _info.simTime; + this->dataPtr->trajectory.status = Trajectory::Active; + } + + // If the new trajectory has no points, consider it reached + if (this->dataPtr->trajectory.points.empty()) + { + this->dataPtr->trajectory.status = Trajectory::Reached; + } + + // Update is always needed for a new trajectory + isTargetUpdateRequired = true; + } + + if (this->dataPtr->trajectory.status == Trajectory::Active) + { + // Determine what point needs to be reached at the current time + if (this->dataPtr->trajectory.UpdateCurrentPoint(_info.simTime)) + { + // Update is needed if point was updated + isTargetUpdateRequired = true; + } + } + + // Update the target for each joint that is defined in the + // trajectory, if needed + if (isTargetUpdateRequired && + this->dataPtr->trajectory.status != Trajectory::Reached) + { + const auto targetPoint = + this->dataPtr->trajectory.points[this->dataPtr->trajectory + .pointIndex]; + for (auto jointIndex = 0u; + jointIndex < this->dataPtr->trajectory.jointNames.size(); + ++jointIndex) + { + const auto jointName = this->dataPtr->trajectory.jointNames[jointIndex]; + if (this->dataPtr->actuatedJoints.count(jointName) == 0) + { + // Warning about unconfigured joint is already logged above + continue; + } + auto *joint = &this->dataPtr->actuatedJoints[jointName]; + joint->SetTarget(targetPoint, jointIndex); + } + + // If there are no more points after the current one, set the trajectory + // to Reached + if (this->dataPtr->trajectory.IsGoalReached()) + { + this->dataPtr->trajectory.status = Trajectory::Reached; + } + + // Publish current progress of the trajectory + ignition::msgs::Float progressMsg; + progressMsg.set_data(this->dataPtr->trajectory.ComputeProgress()); + this->dataPtr->progressPub.Publish(progressMsg); + } + } + + // Control loop + for (auto &actuatedJoint : this->dataPtr->actuatedJoints) + { + auto *joint = &actuatedJoint.second; + joint->Update(_ecm, _info.dt); + } +} + +//////////////////////////////////////// +/// JointTrajectoryControllerPrivate /// +//////////////////////////////////////// + +////////////////////////////////////////////////// +std::vector JointTrajectoryControllerPrivate::GetEnabledJoints( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm) const +{ + std::vector output; + + // Get list of user-enabled joint names. If empty, enable all 1-axis joints + const auto enabledJoints = JointParameters::Parse(_sdf, + "joint_name"); + + // Get list of joint entities of the model + // If there are joints explicitely enabled by the user, get only those + std::vector jointEntities; + if (!enabledJoints.empty()) + { + for (const auto &enabledJointName : enabledJoints) + { + auto enabledJointEntity = _ecm.ChildrenByComponents( + _entity, components::Joint(), components::Name(enabledJointName)); + // Check that model has exactly one joint that matches the name + if (enabledJointEntity.empty()) + { + ignerr << "[JointTrajectoryController] Model does not contain joint [" + << enabledJointName << "], which was explicitly enabled.\n"; + continue; + } + else if (enabledJointEntity.size() > 1) + { + ignwarn << "[JointTrajectoryController] Model has " + << enabledJointEntity.size() << " duplicate joints named [" + << enabledJointName << "]. Only the first (Entity=" + << enabledJointEntity[0] << ") will be configured.\n"; + } + // Add entity to the list of enabled joints + jointEntities.push_back(enabledJointEntity[0]); + } + } + else + { + jointEntities = _ecm.ChildrenByComponents(_entity, components::Joint()); + } + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto &jointEntity : jointEntities) + { + const auto jointName = _ecm.Component( + jointEntity)->Data(); + + // Ignore duplicate joints + for (const auto &actuatedJoint : this->actuatedJoints) + { + if (actuatedJoint.second.entity == jointEntity) + { + ignwarn << "[JointTrajectoryController] Ignoring duplicate joint [" + << jointName << "(Entity=" << jointEntity << ")].\n"; + continue; + } + } + + // Make sure the joint type is supported, i.e. it has exactly one + // actuated axis + const auto *jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) + { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + { + // Supported joint type + break; + } + case sdf::JointType::FIXED: + { + igndbg << "[JointTrajectoryController] Fixed joint [" << jointName + << "(Entity=" << jointEntity << ")] is skipped.\n"; + continue; + } + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: + { + ignwarn << "[JointTrajectoryController] Joint [" << jointName + << "(Entity=" << jointEntity + << ")] is of unsupported type. Only joints with a single axis" + " are supported.\n"; + continue; + } + default: + { + ignwarn << "[JointTrajectoryController] Joint [" << jointName + << "(Entity=" << jointEntity << ")] is of unknown type.\n"; + continue; + } + } + output.push_back(jointEntity); + } + + return output; +} + +////////////////////////////////////////////////// +void JointTrajectoryControllerPrivate::JointTrajectoryCallback( + const ignition::msgs::JointTrajectory &_msg) +{ + // Make sure the message is valid + if (_msg.joint_names_size() == 0) + { + ignwarn << "[JointTrajectoryController] JointTrajectory message does not" + " contain any joint names.\n"; + return; + } + + // Warn user that accelerations are currently ignored if the first point + // contains them + if (_msg.points(0).accelerations_size() > 0) + { + ignwarn << "[JointTrajectoryController] JointTrajectory message contains" + " acceleration commands, which are currently ignored.\n"; + } + + // Lock mutex guarding the trajectory + std::lock_guard lock(this->trajectoryMutex); + + if (this->trajectory.status != Trajectory::Reached) + { + ignwarn << "[JointTrajectoryController] A new JointTrajectory message was" + " received while executing a previous trajectory.\n"; + } + + // Get start time of the trajectory from message header if desired + // If not enabled or there is no header, set start time to 0 and determine + // it later from simTime + if (this->useHeaderStartTime && _msg.has_header()) + { + if (_msg.header().has_stamp()) + { + const auto stamp = _msg.header().stamp(); + this->trajectory.startTime = std::chrono::seconds(stamp.sec()) + + std::chrono::nanoseconds(stamp.nsec()); + } + } + else + { + this->trajectory.startTime = std::chrono::nanoseconds(0); + } + + // Reset for a new trajectory + this->trajectory.Reset(); + + // Extract joint names and points + for (const auto &joint_name : _msg.joint_names()) + { + this->trajectory.jointNames.push_back(joint_name); + } + for (const auto &point : _msg.points()) + { + this->trajectory.points.push_back(point); + } +} + +////////////////////////////////////////////////// +void JointTrajectoryControllerPrivate::Reset() +{ + for (auto &actuatedJoint : this->actuatedJoints) + { + auto *joint = &actuatedJoint.second; + // Reset joint target + joint->ResetTarget(); + // Reset PIDs + joint->ResetPIDs(); + } + + // Reset trajectory + this->trajectory.Reset(); +} + +/////////////////////// +/// JointParameters /// +/////////////////////// + +////////////////////////////////////////////////// +std::map JointParameters::ParseAll( + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + std::vector _enabledJoints) +{ + std::map output; + + const auto initialPositionAll = JointParameters::Parse(_sdf, + "initial_position"); + + const auto positionPGainAll = JointParameters::Parse(_sdf, + "position_p_gain"); + const auto positionIGainAll = JointParameters::Parse(_sdf, + "position_i_gain"); + const auto positionDGainAll = JointParameters::Parse(_sdf, + "position_d_gain"); + const auto positionIMinAll = JointParameters::Parse(_sdf, + "position_i_min"); + const auto positionIMaxAll = JointParameters::Parse(_sdf, + "position_i_max"); + const auto positionCmdMinAll = JointParameters::Parse(_sdf, + "position_cmd_min"); + const auto positionCmdMaxAll = JointParameters::Parse(_sdf, + "position_cmd_max"); + const auto positionCmdOffsetAll = JointParameters::Parse(_sdf, + "position_cmd_offset"); + + const auto velocityPGainAll = JointParameters::Parse(_sdf, + "velocity_p_gain"); + const auto velocityIGainAll = JointParameters::Parse(_sdf, + "velocity_i_gain"); + const auto velocityDGainAll = JointParameters::Parse(_sdf, + "velocity_d_gain"); + const auto velocityIMinAll = JointParameters::Parse(_sdf, + "velocity_i_min"); + const auto velocityIMaxAll = JointParameters::Parse(_sdf, + "velocity_i_max"); + const auto velocityCmdMinAll = JointParameters::Parse(_sdf, + "velocity_cmd_min"); + const auto velocityCmdMaxAll = JointParameters::Parse(_sdf, + "velocity_cmd_max"); + const auto velocityCmdOffsetAll = JointParameters::Parse(_sdf, + "velocity_cmd_offset"); + + for (std::size_t i = 0; i < _enabledJoints.size(); ++i) + { + JointParameters params; + params.initialPosition = params.NthElementOr(initialPositionAll, i, + params.initialPositionDefault); + + params.positionPID.pGain = params.NthElementOr(positionPGainAll, i, + params.positionPID.pGainDefault); + params.positionPID.iGain = params.NthElementOr(positionIGainAll, i, + params.positionPID.iGainDefault); + params.positionPID.dGain = params.NthElementOr(positionDGainAll, i, + params.positionPID.dGainDefault); + params.positionPID.iMin = params.NthElementOr(positionIMinAll, i, + params.positionPID.iMinDefault); + params.positionPID.iMax = params.NthElementOr(positionIMaxAll, i, + params.positionPID.iMaxDefault); + params.positionPID.cmdMin = params.NthElementOr(positionCmdMinAll, i, + params.positionPID.cmdMinDefault); + params.positionPID.cmdMax = params.NthElementOr(positionCmdMaxAll, i, + params.positionPID.cmdMaxDefault); + params.positionPID.cmdOffset = params.NthElementOr(positionCmdOffsetAll, i, + params.positionPID.cmdOffsetDefault); + + params.velocityPID.pGain = params.NthElementOr(velocityPGainAll, i, + params.velocityPID.pGainDefault); + params.velocityPID.iGain = params.NthElementOr(velocityIGainAll, i, + params.velocityPID.iGainDefault); + params.velocityPID.dGain = params.NthElementOr(velocityDGainAll, i, + params.velocityPID.dGainDefault); + params.velocityPID.iMin = params.NthElementOr(velocityIMinAll, i, + params.velocityPID.iMinDefault); + params.velocityPID.iMax = params.NthElementOr(velocityIMaxAll, i, + params.velocityPID.iMaxDefault); + params.velocityPID.cmdMin = params.NthElementOr(velocityCmdMinAll, i, + params.velocityPID.cmdMinDefault); + params.velocityPID.cmdMax = params.NthElementOr(velocityCmdMaxAll, i, + params.velocityPID.cmdMaxDefault); + params.velocityPID.cmdOffset = params.NthElementOr(velocityCmdOffsetAll, i, + params.velocityPID.cmdOffsetDefault); + + const auto jointName = _ecm.Component( + _enabledJoints[i])->Data(); + output[jointName] = params; + } + + return output; +} + +////////////////////////////////////////////////// +template +std::vector JointParameters::Parse( + const std::shared_ptr &_sdf, + const std::string &_parameterName) +{ + std::vector output; + + if (_sdf->HasElement(_parameterName)) + { + sdf::ElementPtr param = const_cast( + _sdf.get())->GetElement(_parameterName); + while (param) + { + output.push_back(param->Get()); + param = param->GetNextElement(_parameterName); + } + } + + return output; +} + +//////////////////////////////////////////////// +template +T JointParameters::NthElementOr(const std::vector &_vec, + const size_t &_index, + const T &_alternative_value) +{ + if (_index < _vec.size()) + { + return _vec[_index]; + } + else + { + return _alternative_value; + } +} + +///////////////////// +/// ActuatedJoint /// +///////////////////// + +////////////////////////////////////////////////// +ActuatedJoint::ActuatedJoint(const Entity &_entity, + const JointParameters &_params) +{ + this->entity = _entity; + + this->initialPosition = _params.initialPosition; + this->target.position = _params.initialPosition; + this->target.velocity = 0.0; + this->target.acceleration = 0.0; + this->target.effort = 0.0; + + this->pids.position = ignition::math::PID(_params.positionPID.pGain, + _params.positionPID.iGain, + _params.positionPID.dGain, + _params.positionPID.iMax, + _params.positionPID.iMin, + _params.positionPID.cmdMax, + _params.positionPID.cmdMin, + _params.positionPID.cmdOffset); + + this->pids.velocity = ignition::math::PID(_params.velocityPID.pGain, + _params.velocityPID.iGain, + _params.velocityPID.dGain, + _params.velocityPID.iMax, + _params.velocityPID.iMin, + _params.velocityPID.cmdMax, + _params.velocityPID.cmdMin, + _params.velocityPID.cmdOffset); + + igndbg << "[JointTrajectoryController] Parameters for joint (Entity=" + << _entity << "):\n" + << "initial_position: [" << _params.initialPosition << "]\n" + << "position_p_gain: [" << _params.positionPID.pGain << "]\n" + << "position_i_gain: [" << _params.positionPID.iGain << "]\n" + << "position_d_gain: [" << _params.positionPID.dGain << "]\n" + << "position_i_min: [" << _params.positionPID.iMax << "]\n" + << "position_i_max: [" << _params.positionPID.iMax << "]\n" + << "position_cmd_min: [" << _params.positionPID.cmdMin << "]\n" + << "position_cmd_max: [" << _params.positionPID.cmdMax << "]\n" + << "position_cmd_offset: [" << _params.positionPID.cmdOffset << "]\n" + << "velocity_p_gain: [" << _params.velocityPID.pGain << "]\n" + << "velocity_i_gain: [" << _params.velocityPID.iGain << "]\n" + << "velocity_d_gain: [" << _params.velocityPID.dGain << "]\n" + << "velocity_i_min: [" << _params.velocityPID.iMax << "]\n" + << "velocity_i_max: [" << _params.velocityPID.iMax << "]\n" + << "velocity_cmd_min: [" << _params.velocityPID.cmdMin << "]\n" + << "velocity_cmd_max: [" << _params.velocityPID.cmdMax << "]\n" + << "velocity_cmd_offset: [" << _params.velocityPID.cmdOffset << "]\n"; +} + +////////////////////////////////////////////////// +void ActuatedJoint::SetupComponents( + ignition::gazebo::EntityComponentManager &_ecm) const +{ + // Create JointPosition component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointPosition()); + } + + // Create JointVelocity component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointVelocity()); + } + + // Create JointForceCmd component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointForceCmd({0.0})); + } +} + +////////////////////////////////////////////////// +void ActuatedJoint::SetTarget( + const ignition::msgs::JointTrajectoryPoint &_targetPoint, + const size_t &_jointIndex) +{ + if ((signed)_jointIndex < _targetPoint.positions_size()) + { + this->target.position = _targetPoint.positions(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.velocities_size()) + { + this->target.velocity = _targetPoint.velocities(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.accelerations_size()) + { + this->target.acceleration = _targetPoint.accelerations(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.effort_size()) + { + this->target.effort = _targetPoint.effort(_jointIndex); + } +} + +////////////////////////////////////////////////// +void ActuatedJoint::Update(ignition::gazebo::EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_dt) +{ + // Get JointPosition and JointVelocity components + const auto jointPositionComponent = _ecm.Component( + this->entity); + const auto jointVelocityComponent = _ecm.Component( + this->entity); + + // Compute control errors and force for each PID controller + double forcePosition = 0.0, forceVelocity = 0.0; + if (!jointPositionComponent->Data().empty()) + { + double errorPosition = jointPositionComponent->Data()[0] - + this->target.position; + forcePosition = this->pids.position.Update(errorPosition, _dt); + } + if (!jointVelocityComponent->Data().empty()) + { + double errorVelocity = jointVelocityComponent->Data()[0] - + this->target.velocity; + forceVelocity = this->pids.velocity.Update(errorVelocity, _dt); + } + + // Sum all forces + const double force = forcePosition + forceVelocity + this->target.effort; + + // Get JointForceCmd component and apply command force + auto jointForceCmdComponent = _ecm.Component( + this->entity); + jointForceCmdComponent->Data()[0] = force; +} + +////////////////////////////////////////////////// +void ActuatedJoint::ResetTarget() +{ + this->target.position = this->initialPosition; + this->target.velocity = 0.0; + this->target.acceleration = 0.0; + this->target.effort = 0.0; +} + +////////////////////////////////////////////////// +void ActuatedJoint::ResetPIDs() +{ + this->pids.position.Reset(); + this->pids.velocity.Reset(); +} + +////////////////// +/// Trajectory /// +////////////////// + +////////////////////////////////////////////////// +bool Trajectory::UpdateCurrentPoint( + const std::chrono::steady_clock::duration &_simTime) +{ + bool isUpdated = false; + + const auto trajectoryTime = _simTime - this->startTime; + while (true) + { + // Break if end of trajectory is reached (there are no more points after + // the current one) + if (this->IsGoalReached()) + { + break; + } + + // Break if point needs to be followed + const auto pointTFS = this->points[this->pointIndex].time_from_start(); + const auto pointTime = std::chrono::seconds(pointTFS.sec()) + + std::chrono::nanoseconds(pointTFS.nsec()); + if (pointTime >= trajectoryTime) + { + break; + } + + // Otherwise increment and try again (joint targets need to be updated) + ++this->pointIndex; + isUpdated = true; + }; + + // Return true if a point index was updated + return isUpdated; +} + +////////////////////////////////////////////////// +bool Trajectory::IsGoalReached() const +{ + return this->pointIndex + 1 >= this->points.size(); +} + +////////////////////////////////////////////////// +float Trajectory::ComputeProgress() const +{ + if (this->points.size() == 0) + { + return 1.0; + } + else + { + return static_cast(this->pointIndex + 1) / + static_cast(this->points.size()); + } +} + +////////////////////////////////////////////////// +void Trajectory::Reset() +{ + this->status = Trajectory::New; + this->pointIndex = 0; + this->jointNames.clear(); + this->points.clear(); +} + +// Register plugin +IGNITION_ADD_PLUGIN(JointTrajectoryController, + ignition::gazebo::System, + JointTrajectoryController::ISystemConfigure, + JointTrajectoryController::ISystemPreUpdate) +IGNITION_ADD_PLUGIN_ALIAS( + JointTrajectoryController, + "ignition::gazebo::systems::JointTrajectoryController") diff --git a/src/systems/joint_trajectory_controller/JointTrajectoryController.hh b/src/systems/joint_trajectory_controller/JointTrajectoryController.hh new file mode 100644 index 0000000000..8f895b77e0 --- /dev/null +++ b/src/systems/joint_trajectory_controller/JointTrajectoryController.hh @@ -0,0 +1,164 @@ +/* + * 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_GAZEBO_SYSTEMS_JOINT_TRAJECTORY_CONTROLLER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_JOINT_TRAJECTORY_CONTROLLER_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class IGNITION_GAZEBO_HIDDEN JointTrajectoryControllerPrivate; + + /// \brief Joint trajectory controller, which can be attached to a model with + /// reference to one or more 1-axis joints in order to follow a trajectory. + /// + /// Joint trajectories can be sent to this plugin via Ignition Transport. + /// The default topic name is "/model/${MODEL_NAME}/joint_trajectory" that + /// can be configured with the `` system parameter. + /// + /// This topic accepts ignition::msgs::JointTrajectory messages that represent + /// a full trajectory, defined as temporal `points` with their fields ordered + /// according to `joint_names` field. The fields under `points` are + /// `positions` - Controlled by position PID controller for each joint + /// `velocities` - Controlled by velocity PID controller for each joint + /// `accelerations` - This field is currently ignored + /// `effort` - Controlled directly for each joint + /// `time_from_start` - Temporal information about the point + /// + /// Forces/torques from `position`, `velocity` and `effort` are summed and + /// applied to the joint. Each PID controller can be configured with system + /// parameters described below. + /// + /// Input trajectory can be produced by a motion planning framework such as + /// MoveIt2. For smooth execution of the trajectory, its points should to be + /// interpolated before sending them via Ignition Transport (interpolation + /// might already be implemented in the motion planning framework of your + /// choice). + /// + /// The progress of the current trajectory can be tracked on topic whose name + /// is derived as `_progress`. This progress is indicated in the range + /// of (0.0, 1.0] and is currently based purely on `time_from_start` contained + /// in the trajectory points. + /// + /// ## System Parameters + /// + /// `` The name of the topic that this plugin subscribes to. + /// Optional parameter. + /// Defaults to "/model/${MODEL_NAME}/joint_trajectory". + /// + /// `` If enabled, trajectory execution begins at the + /// timestamp contained in the header of received trajectory messages. + /// Optional parameter. + /// Defaults to false. + /// + /// `` Name of a joint to control. + /// This parameter can be specified multiple times, i.e. once for each joint. + /// Optional parameter. + /// Defaults to all 1-axis joints contained in the model SDF (order is kept). + /// + /// `` Initial position of a joint (for position control). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// Defaults to 0 for all joints. + /// + /// `<%s_p_gain>` The proportional gain of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_i_gain>` The integral gain of the PID. Optional parameter. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_d_gain>` The derivative gain of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_i_min>` The integral lower limit of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no limit if higher than `%s_i_max`). + /// + /// `<%s_i_max>` The integral upper limit of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is -1 (no limit if lower than `%s_i_min`). + /// + /// `<%s_cmd_min>` Output min value of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no limit if higher than `%s_i_max`). + /// + /// `<%s_cmd_max>` Output max value of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is -1 (no limit if lower than `%s_i_min`). + /// + /// `<%s_cmd_offset>` Command offset (feed-forward) of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no offset). + class IGNITION_GAZEBO_VISIBLE JointTrajectoryController + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: JointTrajectoryController(); + + /// \brief Destructor + public: ~JointTrajectoryController() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 5e2233fbd0..c306ec83bc 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -22,6 +22,7 @@ set(tests joint_controller_system.cc joint_position_controller_system.cc joint_state_publisher_system.cc + joint_trajectory_controller_system.cc kinetic_energy_monitor_system.cc lift_drag_system.cc level_manager.cc diff --git a/test/integration/joint_trajectory_controller_system.cc b/test/integration/joint_trajectory_controller_system.cc new file mode 100644 index 0000000000..70ca0aad3f --- /dev/null +++ b/test/integration/joint_trajectory_controller_system.cc @@ -0,0 +1,399 @@ +/* + * 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 + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointVelocity.hh" +#include "ignition/gazebo/components/Name.hh" + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +#define TOL 1e-4 + +using namespace ignition; +using namespace gazebo; + +/// \brief Test fixture for JointTrajectoryController system +class JointTrajectoryControllerTestFixture : 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); + } +}; + +///////////////////////////////////////////////// +// Tests that JointTrajectoryController accepts position-controlled joint +// trajectory +TEST_F(JointTrajectoryControllerTestFixture, + JointTrajectoryControllerPositionControl) +{ + using namespace std::chrono_literals; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_trajectory_controller.sdf"; + + // Define joints of the model to investigate + const size_t kNumberOfJoints = 2; + const std::string jointNames[kNumberOfJoints] = {"RR_position_control_joint1", + "RR_position_control_joint2"}; + + // Define names of Ignition Transport topics + const std::string trajectoryTopic = + "/model/RR_position_control/joint_trajectory"; + const std::string feedbackTopic = + "/model/RR_position_control/joint_trajectory_feedback"; + + // Define initial joint positions + const std::array initialPositions = {0.0, 0.0}; + + // Define a trajectory to follow + msgs::Duration timeFromStart; + std::vector trajectoryTimes; + std::vector> trajectoryPositions; + // Point1 + timeFromStart.set_sec(0); + timeFromStart.set_nsec(666000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({-0.7854, 0.7854}); + // Point2 + timeFromStart.set_sec(1); + timeFromStart.set_nsec(333000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({1.5708, -0.7854}); + // Point3 + timeFromStart.set_sec(2); + timeFromStart.set_nsec(0); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({3.1416, -1.5708}); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Setup test system + test::Relay testSystem; + double currentPositions[kNumberOfJoints]; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + // Create a JointPosition component for each joint if it doesn't exist + for (const auto &jointName : jointNames) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointName)); + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + } + }); + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get the current position of each joint + for (std::size_t i = 0; i < kNumberOfJoints; ++i) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointNames[i])); + const auto jointPositionComponent = + _ecm.Component(joint); + if (nullptr != jointPositionComponent) + { + currentPositions[i] = jointPositionComponent->Data()[0]; + } + } + }); + server.AddSystem(testSystem.systemPtr); + + // Step few iterations and assert that the initial position is kept + const std::size_t initIters = 10; + server.Run(true, initIters, false); + for (size_t i = 0; i < kNumberOfJoints; ++i) + { + EXPECT_NEAR(currentPositions[i], initialPositions[i], TOL); + } + + // Create new JointTrajectory message based on the defined trajectory + ignition::msgs::JointTrajectory msg; + for (const auto &jointName : jointNames) + { + msg.add_joint_names(jointName); + } + for (size_t i = 0; i < trajectoryPositions.size(); ++i) + { + ignition::msgs::JointTrajectoryPoint point; + + // Set the temporal information for the point + auto time = point.mutable_time_from_start(); + time->set_sec(trajectoryTimes[i].sec()); + time->set_nsec(trajectoryTimes[i].nsec()); + + // Add target positions to the point + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + point.add_positions(trajectoryPositions[i][j]); + } + + // Add point to the trajectory + msg.add_points(); + msg.mutable_points(i)->CopyFrom(point); + } + + // Verify that feedback is strictly increasing and reaches value of 1.0 + size_t count = 0; + std::function feedbackCallback = + [&](const ignition::msgs::Float &_msg) { + count++; + if (trajectoryPositions.size() == count) + { + EXPECT_FLOAT_EQ(_msg.data(), 1.0f); + } + else + { + EXPECT_FLOAT_EQ(_msg.data(), static_cast(count) / + static_cast(trajectoryPositions.size())); + } + }; + transport::Node node; + node.Subscribe(feedbackTopic, feedbackCallback); + + // Publish joint trajectory + auto pub = node.Advertise(trajectoryTopic); + pub.Publish(msg); + + // Wait for message to be published + std::this_thread::sleep_for(100ms); + + // Run the simulation while asserting the target position of all joints at + // each trajectory point + auto previousIterFromStart = 0; + for (size_t i = 0; i < trajectoryPositions.size(); ++i) + { + // Number of iters required to reach time_from_start of the current + // point (1ms step size) + auto iterFromStart = trajectoryTimes[i].sec() * 1000 + + trajectoryTimes[i].nsec() / 1000000; + auto neededIters = iterFromStart - previousIterFromStart; + + // Run the simulation + server.Run(true, neededIters, false); + + // Assert that each joint reached its target position + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + EXPECT_NEAR(currentPositions[j], trajectoryPositions[i][j], TOL); + } + + // Keep track of how many iterations have already passed + previousIterFromStart = iterFromStart; + } +} + +///////////////////////////////////////////////// +// Tests that JointTrajectoryController accepts velocity-controlled joint +// trajectory +TEST_F(JointTrajectoryControllerTestFixture, + JointTrajectoryControllerVelocityControl) +{ + using namespace std::chrono_literals; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_trajectory_controller.sdf"; + + // Define joints of the model to investigate + const size_t kNumberOfJoints = 2; + const std::string jointNames[kNumberOfJoints] = {"RR_velocity_control_joint1", + "RR_velocity_control_joint2"}; + + // Define names of Ignition Transport topics + const std::string trajectoryTopic = "/test_custom_topic/velocity_control"; + const std::string feedbackTopic = + "/test_custom_topic/velocity_control_feedback"; + + // Define initial joint velocities + const std::array initialVelocities = {0.0, 0.0}; + + // Define a trajectory to follow + msgs::Duration timeFromStart; + std::vector trajectoryTimes; + std::vector> trajectoryVelocities; + // Point1 + timeFromStart.set_sec(0); + timeFromStart.set_nsec(500000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryVelocities.push_back({0.5, 0.5}); + // Point2 + timeFromStart.set_sec(1); + timeFromStart.set_nsec(0); + trajectoryTimes.push_back(timeFromStart); + trajectoryVelocities.push_back({-1.0, 1.0}); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Setup test system + test::Relay testSystem; + double currentVelocities[kNumberOfJoints]; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + // Create a JointVelocity component for each joint if it doesn't exist + for (const auto &jointName : jointNames) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointName)); + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointVelocity()); + } + } + }); + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get the current velocity of each joint + for (std::size_t i = 0; i < kNumberOfJoints; ++i) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointNames[i])); + const auto jointVelocityComponent = + _ecm.Component(joint); + if (nullptr != jointVelocityComponent) + { + currentVelocities[i] = jointVelocityComponent->Data()[0]; + } + } + }); + server.AddSystem(testSystem.systemPtr); + + // Step few iterations and assert that the initial velocity is kept + const std::size_t initIters = 10; + server.Run(true, initIters, false); + for (size_t i = 0; i < kNumberOfJoints; ++i) + { + EXPECT_NEAR(currentVelocities[i], initialVelocities[i], TOL); + } + + // Create new JointTrajectory message based on the defined trajectory + ignition::msgs::JointTrajectory msg; + for (const auto &jointName : jointNames) + { + msg.add_joint_names(jointName); + } + for (size_t i = 0; i < trajectoryVelocities.size(); ++i) + { + ignition::msgs::JointTrajectoryPoint point; + + // Set the temporal information for the point + auto time = point.mutable_time_from_start(); + time->set_sec(trajectoryTimes[i].sec()); + time->set_nsec(trajectoryTimes[i].nsec()); + + // Add target velocities to the point + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + point.add_velocities(trajectoryVelocities[i][j]); + } + + // Add point to the trajectory + msg.add_points(); + msg.mutable_points(i)->CopyFrom(point); + } + + // Verify that feedback is strictly increasing and reaches value of 1.0 + size_t count = 0; + std::function feedbackCallback = + [&](const ignition::msgs::Float &_msg) { + count++; + if (trajectoryVelocities.size() == count) + { + EXPECT_FLOAT_EQ(_msg.data(), 1.0f); + } + else + { + EXPECT_FLOAT_EQ(_msg.data(), static_cast(count) / + static_cast(trajectoryVelocities.size())); + } + }; + transport::Node node; + node.Subscribe(feedbackTopic, feedbackCallback); + + // Publish joint trajectory + auto pub = node.Advertise(trajectoryTopic); + pub.Publish(msg); + + // Wait for message to be published + std::this_thread::sleep_for(100ms); + + // Run the simulation while asserting the target velocity of all joints at + // each trajectory point + auto previousIterFromStart = 0; + for (size_t i = 0; i < trajectoryVelocities.size(); ++i) + { + // Number of iters required to reach time_from_start of the current + // point (1ms step size) + auto iterFromStart = trajectoryTimes[i].sec() * 1000 + + trajectoryTimes[i].nsec() / 1000000; + auto neededIters = iterFromStart - previousIterFromStart; + + // Run the simulation + server.Run(true, neededIters, false); + + // Assert that each joint reached its target velocity + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + EXPECT_NEAR(currentVelocities[j], trajectoryVelocities[i][j], TOL); + } + + // Keep track of how many iterations have already passed + previousIterFromStart = iterFromStart; + } +} diff --git a/test/worlds/joint_trajectory_controller.sdf b/test/worlds/joint_trajectory_controller.sdf new file mode 100644 index 0000000000..4aaee68ca5 --- /dev/null +++ b/test/worlds/joint_trajectory_controller.sdf @@ -0,0 +1,420 @@ + + + + + + + + + + + + 0.4 0.4 0.4 + false + + + + + + + + + + 3D View + false + docked + + ogre + scene + 0 0 1 0 1.5708 0 + + + + + + World control + false + false + 50 + 100 + 1 + floating + + + + + + true + true + true + + + + + + World stats + false + false + 250 + 110 + 1 + floating + + + + + + true + true + true + true + + + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + + 0 -0.5 0 0 1.5708 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + + RR_position_control_joint2 + 25 + 400 + 0.1 + -0.5 + 0.5 + -5 + 5 + + RR_position_control_joint1 + 50 + 600 + 0.8 + -1 + 1 + -10 + 10 + + + + + + + 0 0.5 0 0 1.5708 0 + + + world + RR_velocity_control_link0 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0.025 + + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_velocity_control_link0 + RR_velocity_control_link1 + + 1 0 0 + + + 0.02 + + + + 0 0 0.1 0 0 0 + RR_velocity_control_link1 + RR_velocity_control_link2 + + 1 0 0 + + + 0.01 + + + + + + test_custom_topic/velocity_control + + + 0.6 + 175 + -10 + 10 + + 0.1 + 200 + -5 + 5 + + + + + From 0f6c2b7ca3ea45a99e12193550890e9034e9ba65 Mon Sep 17 00:00:00 2001 From: Atharva Pusalkar Date: Mon, 15 Feb 2021 14:17:37 +0530 Subject: [PATCH 6/9] Add SDF topic validity check (#632) * Add SDF topic validity check Signed-off-by: Atharva Pusalkar * Fix SDF topic error message Signed-off-by: Atharva Pusalkar --- src/systems/joint_controller/JointController.cc | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index cd35d55805..9e78162403 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -142,7 +142,16 @@ void JointController::Configure(const Entity &_entity, } if (_sdf->HasElement("topic")) { - topic = _sdf->Get("topic"); + topic = transport::TopicUtils::AsValidTopic( + _sdf->Get("topic")); + + if (topic.empty()) + { + ignerr << "Failed to create topic [" << this->dataPtr->jointName + << "]" << " for joint [" << _sdf->Get("topic") + << "]" << std::endl; + return; + } } this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, this->dataPtr.get()); From 215c8ed22c672107edaedfd5602b876acd2417b7 Mon Sep 17 00:00:00 2001 From: ddengster Date: Wed, 17 Feb 2021 03:55:32 +0800 Subject: [PATCH 7/9] Fix EntityComponentManager race condition (#601) Signed-off-by: ddengster Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- src/gui/GuiRunner.cc | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 6f88a441fe..1925efa8a5 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -79,7 +79,16 @@ void GuiRunner::RequestState() } reqSrv = reqSrvValid; - this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this); + auto advertised = this->node.AdvertisedServices(); + if (std::find(advertised.begin(), advertised.end(), reqSrv) == + advertised.end()) + { + if (!this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this)) + { + ignerr << "Failed to advertise [" << reqSrv << "]" << std::endl; + } + } + ignition::msgs::StringMsg req; req.set_data(reqSrv); @@ -98,7 +107,7 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) return; } - plugin->Update(this->updateInfo, this->ecm); + this->RequestState(); } ///////////////////////////////////////////////// From a5c66591704b8d2a6aab0b8b7c16c56ba208e58c Mon Sep 17 00:00:00 2001 From: Martin Pecka Date: Tue, 16 Feb 2021 20:56:31 +0100 Subject: [PATCH 8/9] Added link to HW-accelerated video recording (#627) Signed-off-by: Martin Pecka Co-authored-by: Michael Carroll --- tutorials/video_recorder.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/tutorials/video_recorder.md b/tutorials/video_recorder.md index 50bbef75d9..47454230cf 100644 --- a/tutorials/video_recorder.md +++ b/tutorials/video_recorder.md @@ -105,3 +105,10 @@ going to the world SDF file, locate the * **bitrate**: Video encoding bitrate in bps. This affects the quality of the generated video. The default bitrate is 2Mbps. + +## Hardware-accelerated encoding + +Since Ignition Common 3.10.2, there is support for utilizing the power of GPUs +to speed up the video encoding process. See the +[Hardware-accelerated Video Encoding tutorial](https://ignitionrobotics.org/api/common/3.10/hw-encoding.html) +for more details. From 5e0c773cb64275604061575333a7384252fd2f31 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen Date: Tue, 16 Feb 2021 14:07:04 -0800 Subject: [PATCH 9/9] Remove issue & PR templates (#631) Signed-off-by: Jenn Nguyen --- .github/ISSUE_TEMPLATE/bug_report.md | 29 ------------- .github/ISSUE_TEMPLATE/feature_request.md | 23 ---------- .github/PULL_REQUEST_TEMPLATE.md | 52 ----------------------- .github/PULL_REQUEST_TEMPLATE/port.md | 6 --- 4 files changed, 110 deletions(-) delete mode 100644 .github/ISSUE_TEMPLATE/bug_report.md delete mode 100644 .github/ISSUE_TEMPLATE/feature_request.md delete mode 100644 .github/PULL_REQUEST_TEMPLATE.md delete mode 100644 .github/PULL_REQUEST_TEMPLATE/port.md diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md deleted file mode 100644 index 3d89f64143..0000000000 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ /dev/null @@ -1,29 +0,0 @@ ---- -name: Bug report -about: Report a bug -labels: bug ---- - - - -## Environment -* OS Version: -* Source or binary build? - - - -## Description -* Expected behavior: -* Actual behavior: - -## Steps to reproduce - - -1. -2. -3. - -## Output - diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md deleted file mode 100644 index f49727a0ce..0000000000 --- a/.github/ISSUE_TEMPLATE/feature_request.md +++ /dev/null @@ -1,23 +0,0 @@ ---- -name: Feature request -about: Request a new feature -labels: enhancement ---- - - - -## Desired behavior - - -## Alternatives considered - - -## Implementation suggestion - - -## Additional context - diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md deleted file mode 100644 index 6c88bd5465..0000000000 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ /dev/null @@ -1,52 +0,0 @@ - - -# Bug Report - -Fixes issue # - -## Summary - - -## Checklist -- [ ] Signed all commits for DCO -- [ ] Added tests -- [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] `codecheck` passed (See - [contributing](https://ignitionrobotics.org/docs/all/contributing#contributing-code)) -- [ ] All tests passed (See - [test coverage](https://ignitionrobotics.org/docs/all/contributing#test-coverage)) -- [ ] While waiting for a review on your PR, please help review -[another open pull request](https://github.com/pulls?q=is%3Aopen+is%3Apr+user%3Aignitionrobotics+archived%3Afalse+) -to support the maintainers - -**Note to maintainers**: Remember to use **Squash-Merge** - ---- - -# New feature - -Closes # - -## Summary - - -## Test it - - -## Checklist -- [ ] Signed all commits for DCO -- [ ] Added tests -- [ ] Added example world and/or tutorial -- [ ] Updated documentation (as needed) -- [ ] Updated migration guide (as needed) -- [ ] `codecheck` passed (See [contributing](https://ignitionrobotics.org/docs/all/contributing#contributing-code)) -- [ ] All tests passed (See [test coverage](https://ignitionrobotics.org/docs/all/contributing#test-coverage)) -- [ ] While waiting for a review on your PR, please help review -[another open pull request](https://github.com/pulls?q=is%3Aopen+is%3Apr+user%3Aignitionrobotics+archived%3Afalse+) -to support the maintainers - -**Note to maintainers**: Remember to use **Squash-Merge** diff --git a/.github/PULL_REQUEST_TEMPLATE/port.md b/.github/PULL_REQUEST_TEMPLATE/port.md deleted file mode 100644 index 758127f585..0000000000 --- a/.github/PULL_REQUEST_TEMPLATE/port.md +++ /dev/null @@ -1,6 +0,0 @@ -Port to - -Branch comparison: https://github.com/ignitionrobotics/ign-gazebo/compare/... - -**Note to maintainers**: Remember to **Merge** with commit (not squash-merge -or rebase)