From 8e8aab6d6101bd40ba1918a9823a905ca38f6a9a Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Thu, 11 Aug 2022 16:47:27 -0500 Subject: [PATCH 01/13] Replace pose in ViewAngle with GzPose (#1641) * Replace pose in ViewAngle with GzPose Signed-off-by: youhy --- src/gui/plugins/view_angle/ViewAngle.qml | 150 ++++------------------- 1 file changed, 23 insertions(+), 127 deletions(-) diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index 311ca12b3e..04859fe73f 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -194,136 +194,32 @@ ToolBar { width: parent.width color: "transparent" - ColumnLayout { - width: parent.width - Text { - text: "Camera Pose" - color: Material.Grey - Layout.row: 4 - Layout.column: 1 - leftPadding: 5 - } - - GridLayout { - width: parent.width - columns: 6 + Text { + text: "Camera Pose" + Layout.fillWidth: true + color: Material.Grey + leftPadding: 5 + font.bold: true + } - Text { - text: "X (m)" - color: "dimgrey" - Layout.row: 4 - Layout.column: 1 - leftPadding: 5 - } - IgnSpinBox { - id: x - Layout.fillWidth: true - Layout.row: 4 - Layout.column: 2 - value: ViewAngle.camPose[0] - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Y (m)" - color: "dimgrey" - Layout.row: 5 - Layout.column: 1 - leftPadding: 5 - } - IgnSpinBox { - id: y - Layout.fillWidth: true - Layout.row: 5 - Layout.column: 2 - value: ViewAngle.camPose[1] - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Z (m)" - color: "dimgrey" - Layout.row: 6 - Layout.column: 1 - leftPadding: 5 - } - IgnSpinBox { - id: z - Layout.fillWidth: true - Layout.row: 6 - Layout.column: 2 - value: ViewAngle.camPose[2] - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } + GzPose { + y: 30 + width: parent.width + Layout.fillWidth: true + readOnly: false + xValue: ViewAngle.camPose[0] + yValue: ViewAngle.camPose[1] + zValue: ViewAngle.camPose[2] + rollValue: ViewAngle.camPose[3] + pitchValue: ViewAngle.camPose[4] + yawValue: ViewAngle.camPose[5] - Text { - text: "Roll (rad)" - color: "dimgrey" - Layout.row: 4 - Layout.column: 3 - leftPadding: 5 - } - IgnSpinBox { - id: roll - Layout.fillWidth: true - Layout.row: 4 - Layout.column: 4 - value: ViewAngle.camPose[3] - maximumValue: 6.28 - minimumValue: -6.28 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Pitch (rad)" - color: "dimgrey" - Layout.row: 5 - Layout.column: 3 - leftPadding: 5 - } - IgnSpinBox { - id: pitch - Layout.fillWidth: true - Layout.row: 5 - Layout.column: 4 - value: ViewAngle.camPose[4] - maximumValue: 6.28 - minimumValue: -6.28 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Yaw (rad)" - color: "dimgrey" - Layout.row: 6 - Layout.column: 3 - leftPadding: 5 - } - IgnSpinBox { - id: yaw - Layout.fillWidth: true - Layout.row: 6 - Layout.column: 4 - value: ViewAngle.camPose[5] - maximumValue: 6.28 - minimumValue: -6.28 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } + onGzPoseSet: { + // _x, _y, _z, _roll, _pitch, _yaw are parameters of signal gzPoseSet + // from gz-gui GzPose.qml + ViewAngle.SetCamPose(_x, _y, _z, _roll, _pitch, _yaw) } + expand: true } } } From 87cabc8797ba09eee0d2e2ff19bc6c68438c36d8 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Fri, 12 Aug 2022 10:55:26 -0500 Subject: [PATCH 02/13] Change CODEOWNERS and maintainer to Michael (#1644) Signed-off-by: Michael Carroll --- .github/CODEOWNERS | 2 +- README.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 400878bf31..5862fc253e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ # More info: # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners -* @chapulina +* @mjcarroll */rendering/* @iche033 src/systems/physics/* @azeey src/systems/sensors/* @iche033 diff --git a/README.md b/README.md index 25e5b3b24b..a985f29053 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # Gazebo Sim : A Robotic Simulator -**Maintainer:** louise AT openrobotics DOT org +**Maintainer:** michael AT openrobotics DOT org [![GitHub open issues](https://img.shields.io/github/issues-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/issues) [![GitHub open pull requests](https://img.shields.io/github/issues-pr-raw/gazebosim/gz-sim.svg)](https://github.com/gazebosim/gz-sim/pulls) From 4602312ec52e7a3cdd441cc9776a990efd00ec54 Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Fri, 12 Aug 2022 15:36:37 -0500 Subject: [PATCH 03/13] Component Inspector with common widget pose plotting (#1607) * implement Pose3d with GzPose Signed-off-by: youhy Co-authored-by: Jenn Nguyen --- .../plugins/component_inspector/Pose3d.qml | 82 +++++++++++-------- 1 file changed, 48 insertions(+), 34 deletions(-) diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index bfb48b4b56..4263ec8d6e 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -90,47 +90,61 @@ Rectangle { color: "transparent" width: parent.width height: gzPoseInstance.height - RowLayout { - id: gzPoseRow - width: parent.width - // Left spacer - Item { - Layout.preferredWidth: margin + indentation + // Content + GzPose { + id: gzPoseInstance + width: parent.width - margin * 2 - indentation + x: margin + indentation + readOnly: { + var isModel = entityType == "model" + return !(isModel) || nestedModel } - // Content - GzPose { - id: gzPoseInstance - Layout.fillWidth: true - - readOnly: { - var isModel = entityType == "model" - return !(isModel) || nestedModel - } + xValue: model.data[0] + yValue: model.data[1] + zValue: model.data[2] + rollValue: model.data[3] + pitchValue: model.data[4] + yawValue: model.data[5] - xValue: model.data[0] - yValue: model.data[1] - zValue: model.data[2] - rollValue: model.data[3] - pitchValue: model.data[4] - yawValue: model.data[5] - - onGzPoseSet: { - // _x, _y, _z, _roll, _pitch, _yaw are parameters of signal gzPoseSet - sendPose(_x, _y, _z, _roll, _pitch, _yaw) - } - - // By default it is closed - expand: false + onGzPoseSet: { + // _x, _y, _z, _roll, _pitch, _yaw are parameters of signal gzPoseSet + // from gz-gui GzPose.qml + sendPose(_x, _y, _z, _roll, _pitch, _yaw) + } - } // end gzPoseInstance + // By default pose widget is collapsed + expand: false - // Right spacer - Item { - Layout.preferredWidth: margin + // plotting + gzPlotEnabled: true + gzPlotMimeDataX: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + ",x," + model.shortName + } + gzPlotMimeDataY: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + ",y," + model.shortName + } + gzPlotMimeDataZ: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + ",z," + model.shortName + } + gzPlotMimeDataRoll: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + ",roll," + model.shortName } - } // end RowLayout + gzPlotMimeDataPitch: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + ",pitch," + model.shortName + } + gzPlotMimeDataYaw: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + ",yaw," + model.shortName + } + + } // end gzPoseInstance } // end Rectangle } // end Column } // end Rectangle From b5dbd52473bcc0ec096729c9f01a10dd65e20bab Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Fri, 12 Aug 2022 15:51:32 -0500 Subject: [PATCH 04/13] Replace plotIcon in ComponentInspector with GzPlotIcon (#1638) * replace PlotIcon with GzPlotIcon for Light.qml * replace PlotIcon with GzPlotIcon for Physics.qml * replace PlotIcon with GzPlotIcon for SphericalCoordinates.qml Signed-off-by: youhy --- src/gui/plugins/component_inspector/Light.qml | 169 +++++------------- .../plugins/component_inspector/Physics.qml | 43 +---- .../SphericalCoordinates.qml | 57 ++---- 3 files changed, 67 insertions(+), 202 deletions(-) diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml index 081b88972d..ab422762ee 100644 --- a/src/gui/plugins/component_inspector/Light.qml +++ b/src/gui/plugins/component_inspector/Light.qml @@ -198,31 +198,12 @@ Rectangle { } Component { - id: plotIcon - Image { - property string componentInfo: "" - source: "plottable_icon.svg" - anchors.top: parent.top - anchors.left: parent.left - - Drag.mimeData: { "text/plain" : (model === null) ? "" : - "Component," + model.entity + "," + model.typeId + "," + - model.dataType + "," + componentInfo + "," + model.shortName + id: gzPlotIcon + GzPlotIcon { + gzMimeData: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + "," + gzComponentInfo + "," + model.shortName } - Drag.dragType: Drag.Automatic - Drag.supportedActions : Qt.CopyAction - Drag.active: dragMouse.drag.active - // a point to drag from - Drag.hotSpot.x: 0 - Drag.hotSpot.y: y - MouseArea { - id: dragMouse - anchors.fill: parent - drag.target: (model === null) ? null : parent - onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) - onReleased: parent.Drag.drop(); - cursorShape: Qt.DragCopyCursor - } } } @@ -325,12 +306,9 @@ Rectangle { Layout.preferredWidth: intensityText.width + indentation*3 Loader { id: loaderIntensity - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "intensity" } - Component.onCompleted: loaderIntensity.item.componentInfo = "intensity" Text { id : intensityText @@ -371,12 +349,9 @@ Rectangle { Layout.preferredWidth: rSpecularText.width + indentation*3 Loader { id: loaderSpecularR - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "specularR" } - Component.onCompleted: loaderSpecularR.item.componentInfo = "specularR" Text { id : rSpecularText @@ -406,12 +381,9 @@ Rectangle { Layout.preferredWidth: gSpecularText.width + indentation*3 Loader { id: loaderSpecularG - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "specularG" } - Component.onCompleted: loaderSpecularG.item.componentInfo = "specularG" Text { id : gSpecularText @@ -443,12 +415,9 @@ Rectangle { Layout.preferredWidth: bSpecularText.width + indentation*3 Loader { id: loaderSpecularB - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "specularB" } - Component.onCompleted: loaderSpecularB.item.componentInfo = "specularB" Text { id : bSpecularText @@ -478,12 +447,9 @@ Rectangle { Layout.preferredWidth: aSpecularText.width + indentation*3 Loader { id: loaderSpecularA - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "specularA" } - Component.onCompleted: loaderSpecularA.item.componentInfo = "specularA" Text { id : aSpecularText @@ -548,12 +514,9 @@ Rectangle { Layout.preferredWidth: rDiffuseText.width + indentation*3 Loader { id: loaderDiffuseR - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "diffuseR" } - Component.onCompleted: loaderDiffuseR.item.componentInfo = "diffuseR" Text { id : rDiffuseText @@ -583,12 +546,9 @@ Rectangle { Layout.preferredWidth: gDiffuseText.width + indentation*3 Loader { id: loaderDiffuseG - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "diffuseG" } - Component.onCompleted: loaderDiffuseG.item.componentInfo = "diffuseG" Text { id : gDiffuseText @@ -620,12 +580,9 @@ Rectangle { Layout.preferredWidth: bDiffuseText.width + indentation*3 Loader { id: loaderDiffuseB - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "diffuseB" } - Component.onCompleted: loaderDiffuseB.item.componentInfo = "diffuseB" Text { id : bDiffuseText @@ -655,12 +612,9 @@ Rectangle { Layout.preferredWidth: aDiffuseText.width + indentation*3 Loader { id: loaderDiffuseA - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "diffuseA" } - Component.onCompleted: loaderDiffuseA.item.componentInfo = "diffuseA" Text { id : aDiffuseText @@ -725,12 +679,9 @@ Rectangle { Layout.preferredWidth: attRangeText.width + indentation*3 Loader { id: loaderAttRange - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "attRange" } - Component.onCompleted: loaderAttRange.item.componentInfo = "attRange" Text { id : attRangeText @@ -762,12 +713,9 @@ Rectangle { Layout.preferredWidth: attLinearText.width + indentation*3 Loader { id: loaderAttLinear - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "attLinear" } - Component.onCompleted: loaderAttLinear.item.componentInfo = "attLinear" Text { id : attLinearText @@ -799,12 +747,9 @@ Rectangle { Layout.preferredWidth: attConstantText.width + indentation*3 Loader { id: loaderAttConstant - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "attConstant" } - Component.onCompleted: loaderAttConstant.item.componentInfo = "attConstant" Text { id : attConstantText @@ -836,12 +781,9 @@ Rectangle { Layout.preferredWidth: attQuadraticText.width + indentation*3 Loader { id: loaderAttQuadratic - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "attQuadratic" } - Component.onCompleted: loaderAttQuadratic.item.componentInfo = "attQuadratic" Text { id : attQuadraticText @@ -873,12 +815,9 @@ Rectangle { Layout.preferredWidth: castShadowsText.width + indentation*3 Loader { id: loaderCastShadows - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "castshadows" } - Component.onCompleted: loaderCastShadows.item.componentInfo = "castshadows" Text { id : castShadowsText @@ -921,12 +860,9 @@ Rectangle { Layout.preferredWidth: xDirectionText.width + indentation*3 Loader { id: loaderDirectionX - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "directionX" } - Component.onCompleted: loaderDirectionX.item.componentInfo = "directionX" Text { visible: model.data[20] === 1 || model.data[20] === 2 @@ -962,12 +898,9 @@ Rectangle { Layout.preferredWidth: yDirectionText.width + indentation*3 Loader { id: loaderDirectionY - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "directionY" } - Component.onCompleted: loaderDirectionY.item.componentInfo = "directionY" Text { visible: model.data[20] === 1 || model.data[20] === 2 @@ -1003,12 +936,9 @@ Rectangle { Layout.preferredWidth: zDirectionText.width + indentation*3 Loader { id: loaderDirectionZ - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "directionZ" } - Component.onCompleted: loaderDirectionZ.item.componentInfo = "directionZ" Text { visible: model.data[20] === 1 || model.data[20] === 2 @@ -1053,12 +983,9 @@ Rectangle { Layout.preferredWidth: innerAngleText.width + indentation*3 Loader { id: loaderInnerAngle - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "innerAngle" } - Component.onCompleted: loaderInnerAngle.item.componentInfo = "innerAngle" Text { visible: model.data[20] === 1 @@ -1094,12 +1021,9 @@ Rectangle { Layout.preferredWidth: outerAngleText.width + indentation*3 Loader { id: loaderOuterAngle - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "outerAngle" } - Component.onCompleted: loaderOuterAngle.item.componentInfo = "outerAngle" Text { visible: model.data[20] === 1 @@ -1135,12 +1059,9 @@ Rectangle { Layout.preferredWidth: fallOffText.width + indentation*3 Loader { id: loaderFallOff - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "falloff" } - Component.onCompleted: loaderFallOff.item.componentInfo = "falloff" Text { visible: model.data[20] === 1 diff --git a/src/gui/plugins/component_inspector/Physics.qml b/src/gui/plugins/component_inspector/Physics.qml index 936aba7cae..9bad794aae 100644 --- a/src/gui/plugins/component_inspector/Physics.qml +++ b/src/gui/plugins/component_inspector/Physics.qml @@ -85,30 +85,11 @@ Rectangle { } Component { - id: plotIcon - Image { - property string componentInfo: "" - source: "plottable_icon.svg" - anchors.top: parent.top - anchors.left: parent.left - - Drag.mimeData: { "text/plain" : (model === null) ? "" : - "Component," + model.entity + "," + model.typeId + "," + - model.dataType + "," + componentInfo + "," + model.shortName - } - Drag.dragType: Drag.Automatic - Drag.supportedActions : Qt.CopyAction - Drag.active: dragMouse.drag.active - // a point to drag from - Drag.hotSpot.x: 0 - Drag.hotSpot.y: y - MouseArea { - id: dragMouse - anchors.fill: parent - drag.target: (model === null) ? null : parent - onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) - onReleased: parent.Drag.drop(); - cursorShape: Qt.DragCopyCursor + id: gzPlotIcon + GzPlotIcon { + gzMimeData: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + "," + gzComponentInfo + "," + model.shortName } } } @@ -156,12 +137,9 @@ Rectangle { Layout.preferredWidth: stepSizeText.width + indentation*3 Loader { id: loaderStepSize - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "stepSize" } - Component.onCompleted: loaderStepSize.item.componentInfo = "stepSize" Text { id : stepSizeText @@ -191,12 +169,9 @@ Rectangle { Layout.preferredWidth: realTimeFactorText.width + indentation*3 Loader { id: loaderRealTimeFactor - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "realTimeFactor" } - Component.onCompleted: loaderRealTimeFactor.item.componentInfo = "realTimeFactor" Text { id : realTimeFactorText diff --git a/src/gui/plugins/component_inspector/SphericalCoordinates.qml b/src/gui/plugins/component_inspector/SphericalCoordinates.qml index 4fe6cb7431..cc1e88dc63 100644 --- a/src/gui/plugins/component_inspector/SphericalCoordinates.qml +++ b/src/gui/plugins/component_inspector/SphericalCoordinates.qml @@ -53,30 +53,11 @@ Rectangle { } Component { - id: plotIcon - Image { - property string componentInfo: "" - source: "plottable_icon.svg" - anchors.top: parent.top - anchors.left: parent.left - - Drag.mimeData: { "text/plain" : (model === null) ? "" : - "Component," + model.entity + "," + model.typeId + "," + - model.dataType + "," + componentInfo + "," + model.shortName - } - Drag.dragType: Drag.Automatic - Drag.supportedActions : Qt.CopyAction - Drag.active: dragMouse.drag.active - // a point to drag from - Drag.hotSpot.x: 0 - Drag.hotSpot.y: y - MouseArea { - id: dragMouse - anchors.fill: parent - drag.target: (model === null) ? null : parent - onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) - onReleased: parent.Drag.drop(); - cursorShape: Qt.DragCopyCursor + id: gzPlotIcon + GzPlotIcon { + gzMimeData: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + "," + gzComponentInfo + "," + model.shortName } } } @@ -163,12 +144,9 @@ Rectangle { Layout.preferredWidth: latText.width + indentation*3 Loader { id: loaderPlotLat - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "latitude" } - Component.onCompleted: loaderPlotLat.item.componentInfo = "latitude" Text { id : latText @@ -201,12 +179,9 @@ Rectangle { Layout.preferredWidth: lonText.width + indentation*3 Loader { id: loaderPlotLon - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "longitude" } - Component.onCompleted: loaderPlotLon.item.componentInfo = "longitude" Text { id : lonText @@ -239,12 +214,9 @@ Rectangle { Layout.preferredWidth: elevationText.width + indentation*3 Loader { id: loaderPlotElevation - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "elevation" } - Component.onCompleted: loaderPlotElevation.item.componentInfo = "elevation" Text { id : elevationText @@ -277,12 +249,9 @@ Rectangle { Layout.preferredWidth: headingText.width + indentation*3 Loader { id: loaderPlotHeading - width: iconWidth - height: iconHeight - y:10 - sourceComponent: plotIcon + sourceComponent: gzPlotIcon + property string gzComponentInfo: "heading" } - Component.onCompleted: loaderPlotHeading.item.componentInfo = "heading" Text { id : headingText From 4d99cfc784ade7bb63018f8244982f1ce8e3b295 Mon Sep 17 00:00:00 2001 From: AzulRadio <50132891+AzulRadio@users.noreply.github.com> Date: Tue, 16 Aug 2022 17:32:45 -0500 Subject: [PATCH 05/13] Common widget GzColor replacement (#1530) * Replace color widgets for Light.qml with GzColor * Replace color widgets for Material.qml with GzColor Signed-off-by: youhy Co-authored-by: Jenn Nguyen --- .../component_inspector/ComponentInspector.cc | 94 +-- src/gui/plugins/component_inspector/Light.qml | 394 ++---------- .../plugins/component_inspector/Material.qml | 606 +++++++----------- 3 files changed, 305 insertions(+), 789 deletions(-) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 4185b826fb..d235fce439 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -318,22 +317,22 @@ void ignition::gazebo::setData(QStandardItem *_item, _item->setData(QString("Material"), ComponentsModel::RoleNames().key("dataType")); _item->setData(QList({ - QVariant(_data.Ambient().R() * 255), - QVariant(_data.Ambient().G() * 255), - QVariant(_data.Ambient().B() * 255), - QVariant(_data.Ambient().A() * 255), - QVariant(_data.Diffuse().R() * 255), - QVariant(_data.Diffuse().G() * 255), - QVariant(_data.Diffuse().B() * 255), - QVariant(_data.Diffuse().A() * 255), - QVariant(_data.Specular().R() * 255), - QVariant(_data.Specular().G() * 255), - QVariant(_data.Specular().B() * 255), - QVariant(_data.Specular().A() * 255), - QVariant(_data.Emissive().R() * 255), - QVariant(_data.Emissive().G() * 255), - QVariant(_data.Emissive().B() * 255), - QVariant(_data.Emissive().A() * 255) + QVariant(_data.Ambient().R()), + QVariant(_data.Ambient().G()), + QVariant(_data.Ambient().B()), + QVariant(_data.Ambient().A()), + QVariant(_data.Diffuse().R()), + QVariant(_data.Diffuse().G()), + QVariant(_data.Diffuse().B()), + QVariant(_data.Diffuse().A()), + QVariant(_data.Specular().R()), + QVariant(_data.Specular().G()), + QVariant(_data.Specular().B()), + QVariant(_data.Specular().A()), + QVariant(_data.Emissive().R()), + QVariant(_data.Emissive().G()), + QVariant(_data.Emissive().B()), + QVariant(_data.Emissive().A()) }), ComponentsModel::RoleNames().key("data")); // TODO(anyone) Only shows colors of material, @@ -1128,55 +1127,8 @@ void ComponentInspector::OnMaterialColor( double _rDiffuse, double _gDiffuse, double _bDiffuse, double _aDiffuse, double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular, double _rEmissive, double _gEmissive, double _bEmissive, double _aEmissive, - QString _type, QColor _currColor) + QString /*_type*/, QColor /*_currColor*/) { - // when type is not empty, open qt color dialog - std::string type = _type.toStdString(); - if (!type.empty()) - { - QColor newColor = QColorDialog::getColor( - _currColor, nullptr, "Pick a color", - {QColorDialog::DontUseNativeDialog, QColorDialog::ShowAlphaChannel}); - - // returns if the user hits cancel - if (!newColor.isValid()) - return; - - if (type == "ambient") - { - _rAmbient = newColor.red(); - _gAmbient = newColor.green(); - _bAmbient = newColor.blue(); - _aAmbient = newColor.alpha(); - } - else if (type == "diffuse") - { - _rDiffuse = newColor.red(); - _gDiffuse = newColor.green(); - _bDiffuse = newColor.blue(); - _aDiffuse = newColor.alpha(); - } - else if (type == "specular") - { - _rSpecular = newColor.red(); - _gSpecular = newColor.green(); - _bSpecular = newColor.blue(); - _aSpecular = newColor.alpha(); - } - else if (type == "emissive") - { - _rEmissive = newColor.red(); - _gEmissive = newColor.green(); - _bEmissive = newColor.blue(); - _aEmissive = newColor.alpha(); - } - else - { - ignerr << "Invalid material type: " << type << std::endl; - return; - } - } - std::function cb = [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) { @@ -1189,17 +1141,13 @@ void ComponentInspector::OnMaterialColor( req.set_id(this->dataPtr->entity); msgs::Set(req.mutable_material()->mutable_ambient(), - math::Color(_rAmbient / 255.0, _gAmbient / 255.0, - _bAmbient / 255.0, _aAmbient / 255.0)); + math::Color(_rAmbient, _gAmbient, _bAmbient, _aAmbient)); msgs::Set(req.mutable_material()->mutable_diffuse(), - math::Color(_rDiffuse / 255.0, _gDiffuse / 255.0, - _bDiffuse / 255.0, _aDiffuse / 255.0)); + math::Color(_rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse)); msgs::Set(req.mutable_material()->mutable_specular(), - math::Color(_rSpecular / 255.0, _gSpecular / 255.0, - _bSpecular / 255.0, _aSpecular / 255.0)); + math::Color(_rSpecular, _gSpecular, _bSpecular, _aSpecular)); msgs::Set(req.mutable_material()->mutable_emissive(), - math::Color(_rEmissive / 255.0, _gEmissive / 255.0, - _bEmissive / 255.0, _aEmissive / 255.0)); + math::Color(_rEmissive, _gEmissive, _bEmissive, _aEmissive)); auto materialCmdService = "/world/" + this->dataPtr->worldName + "/visual_config"; diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml index ab422762ee..f58211db50 100644 --- a/src/gui/plugins/component_inspector/Light.qml +++ b/src/gui/plugins/component_inspector/Light.qml @@ -39,29 +39,17 @@ Rectangle { property int iconWidth: 20 property int iconHeight: 20 - // Loaded item for specular red - property var rSpecularItem: {} - - // Loaded item for specular green - property var gSpecularItem: {} - - // Loaded item for specular blue - property var bSpecularItem: {} - - // Loaded item for specular alpha - property var aSpecularItem: {} + // Loaded item for specular RGBA + property double rSpecularValue: model.data[0] + property double gSpecularValue: model.data[1] + property double bSpecularValue: model.data[2] + property double aSpecularValue: model.data[3] // Loaded item for diffuse red - property var rDiffuseItem: {} - - // Loaded item for diffuse green - property var gDiffuseItem: {} - - // Loaded item for diffuse blue - property var bDiffuseItem: {} - - // Loaded item for diffuse alpha - property var aDiffuseItem: {} + property double rDiffuseValue: model.data[4] + property double gDiffuseValue: model.data[5] + property double bDiffuseValue: model.data[6] + property double aDiffuseValue: model.data[7] // Loaded item for attenuation range property var attRangeItem: {} @@ -109,14 +97,14 @@ Rectangle { function sendLight() { // TODO(anyone) There's a loss of precision when these values get to C++ componentInspector.onLight( - rSpecularItem.value, - gSpecularItem.value, - bSpecularItem.value, - aSpecularItem.value, - rDiffuseItem.value, - gDiffuseItem.value, - bDiffuseItem.value, - aDiffuseItem.value, + rSpecularValue, + gSpecularValue, + bSpecularValue, + aSpecularValue, + rDiffuseValue, + gDiffuseValue, + bDiffuseValue, + aDiffuseValue, attRangeItem.value, attLinearItem.value, attConstantItem.value, @@ -221,7 +209,8 @@ Rectangle { Rectangle { id: content property bool show: false - width: parent.width + x: 10 + width: parent.width - 10 height: show ? grid.height : 0 clip: true color: "transparent" @@ -334,342 +323,93 @@ Rectangle { } } } - RowLayout { - Layout.alignment : Qt.AlignLeft - Text { - text: " Specular" - color: "dimgrey" - width: margin + indentation - } - } - RowLayout { - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: rSpecularText.width + indentation*3 - Loader { - id: loaderSpecularR - sourceComponent: gzPlotIcon - property string gzComponentInfo: "specularR" - } - - Text { - id : rSpecularText - text: ' R' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: rSpecularLoader - anchors.fill: parent - property double numberValue: model.data[0] - sourceComponent: sliderZeroOne - onLoaded: { - rSpecularItem = rSpecularLoader.item - } - } - } - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: gSpecularText.width + indentation*3 - Loader { - id: loaderSpecularG - sourceComponent: gzPlotIcon - property string gzComponentInfo: "specularG" - } - - Text { - id : gSpecularText - text: ' G' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: gSpecularLoader - anchors.fill: parent - property double numberValue: model.data[1] - sourceComponent: sliderZeroOne - onLoaded: { - gSpecularItem = gSpecularLoader.item - } - } - } - } - RowLayout { - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: bSpecularText.width + indentation*3 - Loader { - id: loaderSpecularB - sourceComponent: gzPlotIcon - property string gzComponentInfo: "specularB" - } - - Text { - id : bSpecularText - text: ' B' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: bSpecularLoader - anchors.fill: parent - property double numberValue: model.data[2] - sourceComponent: sliderZeroOne - onLoaded: { - bSpecularItem = bSpecularLoader.item - } - } - } - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: aSpecularText.width + indentation*3 - Loader { - id: loaderSpecularA - sourceComponent: gzPlotIcon - property string gzComponentInfo: "specularA" - } - Text { - id : aSpecularText - text: ' A' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: aSpecularLoader - anchors.fill: parent - property double numberValue: model.data[3] - sourceComponent: sliderZeroOne - onLoaded: { - aSpecularItem = aSpecularLoader.item - } - } - } - } - RowLayout { - Layout.alignment: Qt.AlignHCenter - Button { - Layout.alignment: Qt.AlignHCenter - id: specularColor - text: qsTr("Specular Color") - onClicked: colorDialog.open() - ColorDialog { - id: colorDialog - title: "Choose a specular color" - visible: false - onAccepted: { - rSpecularLoader.item.value = colorDialog.color.r - gSpecularLoader.item.value = colorDialog.color.g - bSpecularLoader.item.value = colorDialog.color.b - aSpecularLoader.item.value = colorDialog.color.a - sendLight() - colorDialog.close() - } - onRejected: { - colorDialog.close() - } - } - } - } RowLayout { + // Color Text { Layout.columnSpan: 6 - text: " Diffuse" + text: " Color" color: "dimgrey" - width: margin + indentation + font.bold: true } } + RowLayout { + // Specular Rectangle { color: "transparent" - height: 40 - Layout.preferredWidth: rDiffuseText.width + indentation*3 - Loader { - id: loaderDiffuseR - sourceComponent: gzPlotIcon - property string gzComponentInfo: "diffuseR" - } - + height: 50 + Layout.preferredWidth: specularText.width + indentation*3 Text { - id : rDiffuseText - text: ' R' + id : specularText + Layout.columnSpan: 2 + text: ' Specular' leftPadding: 5 color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" font.pointSize: 12 anchors.centerIn: parent } } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: rDiffuseLoader - anchors.fill: parent - property double numberValue: model.data[4] - sourceComponent: sliderZeroOne - onLoaded: { - rDiffuseItem = rDiffuseLoader.item - } - } - } - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: gDiffuseText.width + indentation*3 - Loader { - id: loaderDiffuseG - sourceComponent: gzPlotIcon - property string gzComponentInfo: "diffuseG" - } - Text { - id : gDiffuseText - text: ' G' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: gDiffuseLoader - anchors.fill: parent - property double numberValue: model.data[5] - sourceComponent: sliderZeroOne - onLoaded: { - gDiffuseItem = gDiffuseLoader.item - } + // Specular + GzColor { + id: gzColorSpecular + r: model.data[0] + g: model.data[1] + b: model.data[2] + a: model.data[3] + onGzColorSet: { + rSpecularValue = r + gSpecularValue = g + bSpecularValue = b + aSpecularValue = a + sendLight() } } - } - RowLayout { - Rectangle { - color: "transparent" - height: 40 - Layout.preferredWidth: bDiffuseText.width + indentation*3 - Loader { - id: loaderDiffuseB - sourceComponent: gzPlotIcon - property string gzComponentInfo: "diffuseB" - } - Text { - id : bDiffuseText - text: ' B' - leftPadding: 5 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - anchors.centerIn: parent - } - } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: bDiffuseLoader - anchors.fill: parent - property double numberValue: model.data[6] - sourceComponent: sliderZeroOne - onLoaded: { - bDiffuseItem = bDiffuseLoader.item - } - } - } + // Diffuse Rectangle { color: "transparent" - height: 40 - Layout.preferredWidth: aDiffuseText.width + indentation*3 - Loader { - id: loaderDiffuseA - sourceComponent: gzPlotIcon - property string gzComponentInfo: "diffuseA" - } + height: 50 + Layout.preferredWidth: diffuseText.width + indentation*3 Text { - id : aDiffuseText - text: ' A' + id : diffuseText + Layout.columnSpan: 2 + text: ' Diffuse' leftPadding: 5 color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" font.pointSize: 12 anchors.centerIn: parent } } - Item { - Layout.fillWidth: true - height: 40 - Loader { - id: aDiffuseLoader - anchors.fill: parent - property double numberValue: model.data[7] - sourceComponent: sliderZeroOne - onLoaded: { - aDiffuseItem = aDiffuseLoader.item - } - } - } - } - RowLayout { - Layout.alignment: Qt.AlignHCenter - Button { - Layout.alignment: Qt.AlignHCenter - id: diffuseColor - text: qsTr("Diffuse Color") - onClicked: colorDialogDiffuse.open() - ColorDialog { - id: colorDialogDiffuse - title: "Choose a diffuse color" - visible: false - onAccepted: { - rDiffuseLoader.item.value = colorDialogDiffuse.color.r - gDiffuseLoader.item.value = colorDialogDiffuse.color.g - bDiffuseLoader.item.value = colorDialogDiffuse.color.b - aDiffuseLoader.item.value = colorDialogDiffuse.color.a - sendLight() - colorDialogDiffuse.close() - } - onRejected: { - colorDialogDiffuse.close() - } + + // Diffuse + GzColor { + id: gzColorDiffuse + r: model.data[4] + g: model.data[5] + b: model.data[6] + a: model.data[7] + onGzColorSet: { + rDiffuseValue = r + gDiffuseValue = g + bDiffuseValue = b + aDiffuseValue = a + sendLight() } } } + RowLayout { Text { + Layout.topMargin: 10 + Layout.bottomMargin: 10 Layout.columnSpan: 6 text: " Attenuation" color: "dimgrey" width: margin + indentation + font.bold: true } } RowLayout { @@ -850,6 +590,7 @@ Rectangle { text: " Direction" color: "dimgrey" width: margin + indentation + font.bold: true } } RowLayout { @@ -973,6 +714,7 @@ Rectangle { text: " Spot features" color: "dimgrey" width: margin + indentation + font.bold: true } } RowLayout { diff --git a/src/gui/plugins/component_inspector/Material.qml b/src/gui/plugins/component_inspector/Material.qml index a7d82cc6c8..5f281b0585 100644 --- a/src/gui/plugins/component_inspector/Material.qml +++ b/src/gui/plugins/component_inspector/Material.qml @@ -40,50 +40,37 @@ Rectangle { property int iconHeight: 20 // Loaded items for ambient red, green, blue, alpha - property var rAmbientItem: {} - property var gAmbientItem: {} - property var bAmbientItem: {} - property var aAmbientItem: {} + property double rAmbientItem: model.data[0] + property double gAmbientItem: model.data[1] + property double bAmbientItem: model.data[2] + property double aAmbientItem: model.data[3] // Loaded items for diffuse red, green, blue, alpha - property var rDiffuseItem: {} - property var gDiffuseItem: {} - property var bDiffuseItem: {} - property var aDiffuseItem: {} + property double rDiffuseItem: model.data[4] + property double gDiffuseItem: model.data[5] + property double bDiffuseItem: model.data[6] + property double aDiffuseItem: model.data[7] // Loaded items for specular red, green, blue, alpha - property var rSpecularItem: {} - property var gSpecularItem: {} - property var bSpecularItem: {} - property var aSpecularItem: {} + property double rSpecularItem: model.data[8] + property double gSpecularItem: model.data[9] + property double bSpecularItem: model.data[10] + property double aSpecularItem: model.data[11] // Loaded items for emissive red, green, blue, alpha - property var rEmissiveItem: {} - property var gEmissiveItem: {} - property var bEmissiveItem: {} - property var aEmissiveItem: {} + property double rEmissiveItem: model.data[12] + property double gEmissiveItem: model.data[13] + property double bEmissiveItem: model.data[14] + property double aEmissiveItem: model.data[15] // send new material color data to C++ - function sendMaterialColor(_type, _currColor) { + function sendMaterialColor() { componentInspector.onMaterialColor( - rAmbientItem.value, - gAmbientItem.value, - bAmbientItem.value, - aAmbientItem.value, - rDiffuseItem.value, - gDiffuseItem.value, - bDiffuseItem.value, - aDiffuseItem.value, - rSpecularItem.value, - gSpecularItem.value, - bSpecularItem.value, - aSpecularItem.value, - rEmissiveItem.value, - gEmissiveItem.value, - bEmissiveItem.value, - aEmissiveItem.value, - _type, - _currColor + rAmbientItem, gAmbientItem, bAmbientItem, aAmbientItem, + rDiffuseItem, gDiffuseItem, bDiffuseItem, aDiffuseItem, + rSpecularItem, gSpecularItem, bSpecularItem, aSpecularItem, + rEmissiveItem, gEmissiveItem, bEmissiveItem, aEmissiveItem, + "", Qt.rgba(0, 0, 0, 0) // dummy placeholders (no longer needed) ); } @@ -114,19 +101,9 @@ Rectangle { font.family: "Roboto" } - // Used to create rgba spin boxes Component { - id: spinBoxMaterialColor - IgnSpinBox { - id: writableSpin - value: writableSpin.activeFocus ? writableSpin.value : numberValue - minimumValue: 0 - maximumValue: 255 - decimals: 0 - onEditingFinished: { - // sending empty params to not open color dialog - sendMaterialColor("", Qt.rgba(0, 0, 0, 0)) - } + id: gzcolor + GzColor { } } @@ -198,408 +175,257 @@ Rectangle { width: parent.width spacing: 20 - GridLayout { - width: parent.width - columns: 6 - // rgba headers - Text { - text: "Red " - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - Layout.row: 1 - Layout.column: 3 - } - Text { - text: "Green" - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - Layout.row: 1 - Layout.column: 4 - } + RowLayout { + // Color Text { - text: "Blue " - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - Layout.row: 1 - Layout.column: 5 - } - Text { - text: "Alpha" - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - font.pointSize: 12 - Layout.row: 1 - Layout.column: 6 + Layout.topMargin: 10 + Layout.columnSpan: 6 + text: " Color" + color: "dimgrey" + font.bold: true } + } + RowLayout { // Ambient - Text { - text: " Ambient" - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - Layout.row: 2 - Layout.column: 1 - } - // Ambient color dialog - Button { - id: ambientButton - Layout.row: 2 - Layout.column: 2 - ToolTip.text: "Open color dialog" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - background: Rectangle { - implicitWidth: 40 - implicitHeight: 40 - radius: 5 - border.color: getButtonColor(0, false) - border.width: 2 - color: getButtonColor(0, true) - } - onClicked: { - sendMaterialColor("ambient", getButtonColor(0, true)) + Rectangle { + color: "transparent" + height: 50 + Layout.preferredWidth: ambientText.width + indentation*3 + + Text { + id : ambientText + Layout.columnSpan: 2 + text: ' Ambient' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent } } - // Ambient red + + // Ambient Item { - Layout.row: 2 - Layout.column: 3 Layout.fillWidth: true + Layout.bottomMargin: 10 height: 40 Loader { - id: rAmbientLoader + id: ambientLoader anchors.fill: parent - property double numberValue: model.data[0] - sourceComponent: spinBoxMaterialColor - onLoaded: { - rAmbientItem = rAmbientLoader.item - } + sourceComponent: gzcolor } - } - // Ambient green - Item { - Layout.row: 2 - Layout.column: 4 - Layout.fillWidth: true - height: 40 - Loader { - id: gAmbientLoader - anchors.fill: parent - property double numberValue: model.data[1] - sourceComponent: spinBoxMaterialColor - onLoaded: { - gAmbientItem = gAmbientLoader.item - } + Binding { + target: ambientLoader.item + property: "r" + value: model.data[0] } - } - // Ambient blue - Item { - Layout.row: 2 - Layout.column: 5 - Layout.fillWidth: true - height: 40 - Loader { - id: bAmbientLoader - anchors.fill: parent - property double numberValue: model.data[2] - sourceComponent: spinBoxMaterialColor - onLoaded: { - bAmbientItem = bAmbientLoader.item - } + Binding { + target: ambientLoader.item + property: "g" + value: model.data[1] } - } - // Ambient alpha - Item { - Layout.row: 2 - Layout.column: 6 - Layout.fillWidth: true - height: 40 - Loader { - id: aAmbientLoader - anchors.fill: parent - property double numberValue: model.data[3] - sourceComponent: spinBoxMaterialColor - onLoaded: { - aAmbientItem = aAmbientLoader.item + Binding { + target: ambientLoader.item + property: "b" + value: model.data[2] + } + Binding { + target: ambientLoader.item + property: "a" + value: model.data[3] + } + Connections { + target: ambientLoader.item + onGzColorSet: { + rAmbientItem = ambientLoader.item.r + gAmbientItem = ambientLoader.item.g + bAmbientItem = ambientLoader.item.b + aAmbientItem = ambientLoader.item.a + sendMaterialColor() } } - } // end Ambient + } // Diffuse - Text { - text: " Diffuse" - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - Layout.row: 3 - Layout.column: 1 - } - // Diffuse color dialog - Button { - id: diffuseButton - Layout.row: 3 - Layout.column: 2 - ToolTip.text: "Open color dialog" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - background: Rectangle { - implicitWidth: 40 - implicitHeight: 40 - radius: 5 - border.color: getButtonColor(4, false) - border.width: 2 - color: getButtonColor(4, true) - } - onClicked: { - sendMaterialColor("diffuse", getButtonColor(4, true)) + Rectangle { + color: "transparent" + height: 50 + Layout.preferredWidth: diffuseText.width + indentation*3 + + Text { + id : diffuseText + Layout.columnSpan: 2 + text: ' Diffuse' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent } } - // Diffuse red + + // Diffuse Item { - Layout.row: 3 - Layout.column: 3 Layout.fillWidth: true + Layout.bottomMargin: 10 height: 40 Loader { - id: rDiffuseLoader + id: diffuseLoader anchors.fill: parent - property double numberValue: model.data[4] - sourceComponent: spinBoxMaterialColor - onLoaded: { - rDiffuseItem = rDiffuseLoader.item - } + sourceComponent: gzcolor } - } - // Diffuse green - Item { - Layout.row: 3 - Layout.column: 4 - Layout.fillWidth: true - height: 40 - Loader { - id: gDiffuseLoader - anchors.fill: parent - property double numberValue: model.data[5] - sourceComponent: spinBoxMaterialColor - onLoaded: { - gDiffuseItem = gDiffuseLoader.item - } + Binding { + target: diffuseLoader.item + property: "r" + value: model.data[4] } - } - // Diffuse blue - Item { - Layout.row: 3 - Layout.column: 5 - Layout.fillWidth: true - height: 40 - Loader { - id: bDiffuseLoader - anchors.fill: parent - property double numberValue: model.data[6] - sourceComponent: spinBoxMaterialColor - onLoaded: { - bDiffuseItem = bDiffuseLoader.item - } + Binding { + target: diffuseLoader.item + property: "g" + value: model.data[5] } - } - // Diffuse alpha - Item { - Layout.row: 3 - Layout.column: 6 - Layout.fillWidth: true - height: 40 - Loader { - id: aDiffuseLoader - anchors.fill: parent - property double numberValue: model.data[7] - sourceComponent: spinBoxMaterialColor - onLoaded: { - aDiffuseItem = aDiffuseLoader.item + Binding { + target: diffuseLoader.item + property: "b" + value: model.data[6] + } + Binding { + target: diffuseLoader.item + property: "a" + value: model.data[7] + } + Connections { + target: diffuseLoader.item + onGzColorSet: { + rDiffuseItem = diffuseLoader.item.r + gDiffuseItem = diffuseLoader.item.g + bDiffuseItem = diffuseLoader.item.b + aDiffuseItem = diffuseLoader.item.a + sendMaterialColor() } } - } // end Diffuse + } + } + RowLayout { // Specular - Text { - text: " Specular" - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - Layout.row: 4 - Layout.column: 1 - } - // Specular color dialog - Button { - id: specularButton - Layout.row: 4 - Layout.column: 2 - ToolTip.text: "Open color dialog" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - background: Rectangle { - implicitWidth: 40 - implicitHeight: 40 - radius: 5 - border.color: getButtonColor(8, false) - border.width: 2 - color: getButtonColor(8, true) - } - onClicked: { - sendMaterialColor("specular", getButtonColor(8, true)) + Rectangle { + color: "transparent" + height: 50 + Layout.preferredWidth: specularText.width + indentation*3 + + Text { + id : specularText + Layout.columnSpan: 2 + text: ' Specular' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent } } - // Specular red + + // Specular Item { - Layout.row: 4 - Layout.column: 3 Layout.fillWidth: true + Layout.bottomMargin: 10 height: 40 Loader { - id: rSpecularLoader + id: specularLoader anchors.fill: parent - property double numberValue: model.data[8] - sourceComponent: spinBoxMaterialColor - onLoaded: { - rSpecularItem = rSpecularLoader.item - } + sourceComponent: gzcolor } - } - // Specular green - Item { - Layout.row: 4 - Layout.column: 4 - Layout.fillWidth: true - height: 40 - Loader { - id: gSpecularLoader - anchors.fill: parent - property double numberValue: model.data[9] - sourceComponent: spinBoxMaterialColor - onLoaded: { - gSpecularItem = gSpecularLoader.item - } + Binding { + target: specularLoader.item + property: "r" + value: model.data[8] } - } - // Specular blue - Item { - Layout.row: 4 - Layout.column: 5 - Layout.fillWidth: true - height: 40 - Loader { - id: bSpecularLoader - anchors.fill: parent - property double numberValue: model.data[10] - sourceComponent: spinBoxMaterialColor - onLoaded: { - bSpecularItem = bSpecularLoader.item - } + Binding { + target: specularLoader.item + property: "g" + value: model.data[9] } - } - // Specular alpha - Item { - Layout.row: 4 - Layout.column: 6 - Layout.fillWidth: true - height: 40 - Loader { - id: aSpecularLoader - anchors.fill: parent - property double numberValue: model.data[11] - sourceComponent: spinBoxMaterialColor - onLoaded: { - aSpecularItem = aSpecularLoader.item + Binding { + target: specularLoader.item + property: "b" + value: model.data[10] + } + Binding { + target: specularLoader.item + property: "a" + value: model.data[11] + } + Connections { + target: specularLoader.item + onGzColorSet: { + rSpecularItem = specularLoader.item.r + gSpecularItem = specularLoader.item.g + bSpecularItem = specularLoader.item.b + aSpecularItem = specularLoader.item.a + sendMaterialColor() } } - } // end Specular + } // Emissive - Text { - text: " Emissive" - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - Layout.row: 5 - Layout.column: 1 - } - // Emissive color dialog - Button { - id: emissiveButton - Layout.row: 5 - Layout.column: 2 - ToolTip.text: "Open color dialog" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval - background: Rectangle { - implicitWidth: 40 - implicitHeight: 40 - radius: 5 - border.color: getButtonColor(12, false) - border.width: 2 - color: getButtonColor(12, true) - } - onClicked: { - sendMaterialColor("emissive", getButtonColor(12, true)) + Rectangle { + color: "transparent" + height: 50 + Layout.preferredWidth: emissiveText.width + indentation*3 + + Text { + id : emissiveText + Layout.columnSpan: 2 + text: 'Emissive' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent } } - // Emissive red + + // Emissive Item { - Layout.row: 5 - Layout.column: 3 Layout.fillWidth: true + Layout.bottomMargin: 10 height: 40 Loader { - id: rEmissiveLoader + id: emissiveLoader anchors.fill: parent - property double numberValue: model.data[12] - sourceComponent: spinBoxMaterialColor - onLoaded: { - rEmissiveItem = rEmissiveLoader.item - } + sourceComponent: gzcolor } - } - // Emissive green - Item { - Layout.row: 5 - Layout.column: 4 - Layout.fillWidth: true - height: 40 - Loader { - id: gEmissiveLoader - anchors.fill: parent - property double numberValue: model.data[13] - sourceComponent: spinBoxMaterialColor - onLoaded: { - gEmissiveItem = gEmissiveLoader.item - } + Binding { + target: emissiveLoader.item + property: "r" + value: model.data[12] } - } - // Emissive blue - Item { - Layout.row: 5 - Layout.column: 5 - Layout.fillWidth: true - height: 40 - Loader { - id: bEmissiveLoader - anchors.fill: parent - property double numberValue: model.data[14] - sourceComponent: spinBoxMaterialColor - onLoaded: { - bEmissiveItem = bEmissiveLoader.item - } + Binding { + target: emissiveLoader.item + property: "g" + value: model.data[13] } - } - // Emissive alpha - Item { - Layout.row: 5 - Layout.column: 6 - Layout.fillWidth: true - height: 40 - Loader { - id: aEmissiveLoader - anchors.fill: parent - property double numberValue: model.data[15] - sourceComponent: spinBoxMaterialColor - onLoaded: { - aEmissiveItem = aEmissiveLoader.item + Binding { + target: emissiveLoader.item + property: "b" + value: model.data[14] + } + Binding { + target: emissiveLoader.item + property: "a" + value: model.data[15] + } + Connections { + target: emissiveLoader.item + onGzColorSet: { + rEmissiveItem = emissiveLoader.item.r + gEmissiveItem = emissiveLoader.item.g + bEmissiveItem = emissiveLoader.item.b + aEmissiveItem = emissiveLoader.item.a + sendMaterialColor() } } - } // end Emissive - } // end GridLayout + } + } // end RowLayout } // end ColumnLayout (id: grid) } // Rectangle (id: content) } From cd97cdfafaeba2f78ac1155a823455a2e6a85433 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 16 Aug 2022 15:40:08 -0700 Subject: [PATCH 06/13] Add support for specifying log record period (#1636) By specifying the log record period, the user is able to control how often the simulator records states. A new command line arg --record-period is added (gazebo-classic also has the same arg name) Signed-off-by: Ian Chen --- .../ignition/gazebo/EntityComponentManager.hh | 5 ++ include/ignition/gazebo/ServerConfig.hh | 9 +++ src/EntityComponentManager.cc | 6 ++ src/EntityComponentManager_TEST.cc | 5 ++ src/ServerConfig.cc | 28 +++++++ src/ServerConfig_TEST.cc | 5 ++ src/ServerPrivate.cc | 5 ++ src/cmd/cmdgazebo.rb.in | 13 ++- src/cmd/gazebo.bash_completion.sh | 1 + src/ign.cc | 10 ++- src/ign.hh | 4 +- src/systems/log/LogRecord.cc | 38 ++++++++- .../scene_broadcaster/SceneBroadcaster.cc | 46 ++++++++++- test/integration/log_system.cc | 81 +++++++++++++++++++ 14 files changed, 242 insertions(+), 14 deletions(-) diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index d078dbf3a0..8340862482 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -590,6 +590,11 @@ namespace ignition /// \return True if there are any components with one-time changes. public: bool HasOneTimeComponentChanges() const; + /// \brief Get whether there are periodic component changes. These changes + /// may happen frequently and are processed periodically. + /// \return True if there are any components with periodic changes. + public: bool HasPeriodicComponentChanges() const; + /// \brief Get the components types that are marked as periodic changes. /// \return All the components that at least one entity marked as /// periodic changes. diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 74927d6cef..ff4941b41d 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -313,6 +313,15 @@ namespace ignition /// \param[in] _recordPath Path to place recorded states public: void SetLogRecordPath(const std::string &_recordPath); + /// \brief Get time period to record states + /// \return Time period to record states + public: std::chrono::steady_clock::duration LogRecordPeriod() const; + + /// \brief Set time period to record states + /// \param[in] _period Time period to record states + public: void SetLogRecordPeriod( + const std::chrono::steady_clock::duration &_period); + /// \brief Add a topic to record. /// \param[in] _topic Topic name, which can include wildcards. public: void AddLogRecordTopic(const std::string &_topic); diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index df3a1ee56a..b5f0eb8310 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -945,6 +945,12 @@ bool EntityComponentManager::HasOneTimeComponentChanges() const return !this->dataPtr->oneTimeChangedComponents.empty(); } +///////////////////////////////////////////////// +bool EntityComponentManager::HasPeriodicComponentChanges() const +{ + return !this->dataPtr->periodicChangedComponents.empty(); +} + ///////////////////////////////////////////////// std::unordered_set EntityComponentManager::ComponentTypesWithPeriodicChanges() const diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 01b5866e3f..ac8cb416ab 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2220,6 +2220,7 @@ TEST_P(EntityComponentManagerFixture, ASSERT_NE(nullptr, c2); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + EXPECT_FALSE(manager.HasPeriodicComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::OneTimeChange, manager.ComponentState(e1, c1->TypeId())); @@ -2232,6 +2233,7 @@ TEST_P(EntityComponentManagerFixture, // updated manager.RunSetAllComponentsUnchanged(); EXPECT_FALSE(manager.HasOneTimeComponentChanges()); + EXPECT_FALSE(manager.HasPeriodicComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, c1->TypeId())); @@ -2246,6 +2248,7 @@ TEST_P(EntityComponentManagerFixture, EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, c1->TypeId())); EXPECT_FALSE(manager.HasOneTimeComponentChanges()); + EXPECT_FALSE(manager.HasPeriodicComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(0, manager.ChangedState().entities_size()); @@ -2273,6 +2276,7 @@ TEST_P(EntityComponentManagerFixture, EXPECT_TRUE(manager.HasOneTimeComponentChanges()); // Expect a single component type to be marked as PeriodicChange + EXPECT_TRUE(manager.HasPeriodicComponentChanges()); ASSERT_EQ(1u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(IntComponent().TypeId(), *manager.ComponentTypesWithPeriodicChanges().begin()); @@ -2285,6 +2289,7 @@ TEST_P(EntityComponentManagerFixture, EXPECT_TRUE(manager.RemoveComponent(e1, c1->TypeId())); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + EXPECT_FALSE(manager.HasPeriodicComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, c1->TypeId())); diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 00e31e9e8b..ba50a7369c 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -269,6 +269,7 @@ class ignition::gazebo::ServerConfigPrivate useLevels(_cfg->useLevels), useLogRecord(_cfg->useLogRecord), logRecordPath(_cfg->logRecordPath), + logRecordPeriod(_cfg->logRecordPeriod), logPlaybackPath(_cfg->logPlaybackPath), logRecordResources(_cfg->logRecordResources), logRecordCompressPath(_cfg->logRecordCompressPath), @@ -301,6 +302,9 @@ class ignition::gazebo::ServerConfigPrivate /// \brief Path to place recorded states public: std::string logRecordPath = ""; + /// \brief Time period to record states + public: std::chrono::steady_clock::duration logRecordPeriod{0}; + /// \brief Path to recorded states to play back using logging system public: std::string logPlaybackPath = ""; @@ -498,6 +502,19 @@ void ServerConfig::SetLogRecordPath(const std::string &_recordPath) this->dataPtr->logRecordPath = _recordPath; } +///////////////////////////////////////////////// +std::chrono::steady_clock::duration ServerConfig::LogRecordPeriod() const +{ + return this->dataPtr->logRecordPeriod; +} + +///////////////////////////////////////////////// +void ServerConfig::SetLogRecordPeriod( + const std::chrono::steady_clock::duration &_period) +{ + this->dataPtr->logRecordPeriod = _period; +} + ///////////////////////////////////////////////// const std::string ServerConfig::LogPlaybackPath() const { @@ -690,6 +707,17 @@ ServerConfig::LogRecordPlugin() const plugin.InsertContent(topicElem); } + if (this->LogRecordPeriod() > std::chrono::steady_clock::duration::zero()) + { + sdf::ElementPtr periodElem = std::make_shared(); + periodElem->SetName("record_period"); + periodElem->AddValue("double", "0", false, ""); + double t = std::chrono::duration_cast( + this->LogRecordPeriod()).count() * 1e-3; + periodElem->Set(t); + plugin.InsertContent(periodElem); + } + igndbg << plugin.ToElement()->ToString("") << std::endl; return ServerConfig::PluginInfo(entityName, diff --git a/src/ServerConfig_TEST.cc b/src/ServerConfig_TEST.cc index 7e5df2cd25..4e7b7c9aa4 100644 --- a/src/ServerConfig_TEST.cc +++ b/src/ServerConfig_TEST.cc @@ -236,6 +236,11 @@ TEST(ServerConfig, GenerateRecordPlugin) config.SetUseLogRecord(true); config.SetLogRecordPath("foo/bar"); config.SetLogRecordResources(true); + auto period = + std::chrono::duration_cast( + std::chrono::duration(0.04)); + config.SetLogRecordPeriod(period); + EXPECT_EQ(period, config.LogRecordPeriod()); auto plugin = config.LogRecordPlugin(); EXPECT_EQ(plugin.EntityName(), "*"); diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index ef792687dc..0381c96660 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -252,6 +252,11 @@ void ServerPrivate::AddRecordPlugin(const ServerConfig &_config) this->config.SetLogRecordPath(_config.LogRecordPath()); } + if (_config.LogRecordPeriod() > std::chrono::steady_clock::duration::zero()) + { + this->config.SetLogRecordPeriod(_config.LogRecordPeriod()); + } + if (_config.LogRecordResources()) this->config.SetLogRecordResources(true); diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 9944affb28..16ee5e1cac 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -98,6 +98,9 @@ COMMANDS = { 'gazebo' => " --record-topic /stats \ \n"\ " --record-topic /clock \n"\ "\n"\ + " --record-period [arg] Specify the time period (seconds) between \n"\ + " state recording. \n"\ + "\n"\ " --log-overwrite When recording, overwrite existing files. \n"\ " Only valid if recording is enabled. \n"\ "\n"\ @@ -213,6 +216,7 @@ class Cmd 'record-path' => '', 'record-resources' => 0, 'record-topics' => [], + 'record-period' => -1, 'log-overwrite' => 0, 'log-compress' => 0, 'playback' => '', @@ -277,6 +281,9 @@ class Cmd opts.on('--record-topic [arg]', String) do |t| options['record-topics'].append(t) end + opts.on('--record-period [arg]', Float) do |d| + options['record-period'] = d + end opts.on('--log-overwrite') do options['log-overwrite'] = 1 end @@ -438,7 +445,7 @@ has properly set the DYLD_LIBRARY_PATH environment variables." const char *, int, int, const char *, int, int, int, const char *, const char *, const char *, const char *, const char *, - const char *, int, int)' + const char *, int, int, float)' # Import the runGui function Importer.extern 'int runGui(const char *, const char *, int, const char *)' @@ -473,7 +480,7 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." options['render_engine_server'], options['render_engine_gui'], options['file'], options['record-topics'].join(':'), options['wait_gui'], - options['headless-rendering']) + options['headless-rendering'], options['record-period']) end guiPid = Process.fork do @@ -510,7 +517,7 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." options['playback'], options['physics_engine'], options['render_engine_server'], options['render_engine_gui'], options['file'], options['record-topics'].join(':'), - options['wait_gui'], options['headless-rendering']) + options['wait_gui'], options['headless-rendering'], options['record-period']) # Otherwise run the gui else options['gui'] if plugin.end_with? ".dylib" diff --git a/src/cmd/gazebo.bash_completion.sh b/src/cmd/gazebo.bash_completion.sh index 72ad2ac6ff..8f3903f8ef 100644 --- a/src/cmd/gazebo.bash_completion.sh +++ b/src/cmd/gazebo.bash_completion.sh @@ -30,6 +30,7 @@ GZ_SIM_COMPLETION_LIST=" --record-path --record-resources --record-topic + --record-period --log-overwrite --log-compress --playback diff --git a/src/ign.cc b/src/ign.cc index e80baccbc5..76344cf872 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -135,7 +135,7 @@ extern "C" int runServer(const char *_sdfString, const char *_playback, const char *_physicsEngine, const char *_renderEngineServer, const char *_renderEngineGui, const char *_file, const char *_recordTopics, int _waitGui, - int _headless) + int _headless, float _recordPeriod) { std::string startingWorldPath{""}; ignition::gazebo::ServerConfig serverConfig; @@ -182,7 +182,7 @@ extern "C" int runServer(const char *_sdfString, // Initialize console log if ((_recordPath != nullptr && std::strlen(_recordPath) > 0) || - _record > 0 || _recordResources > 0 || + _record > 0 || _recordResources > 0 || _recordPeriod >= 0 || (_recordTopics != nullptr && std::strlen(_recordTopics) > 0)) { if (_playback != nullptr && std::strlen(_playback) > 0) @@ -193,6 +193,12 @@ extern "C" int runServer(const char *_sdfString, serverConfig.SetUseLogRecord(true); serverConfig.SetLogRecordResources(_recordResources); + if (_recordPeriod >= 0) + { + serverConfig.SetLogRecordPeriod( + std::chrono::duration_cast( + std::chrono::duration(_recordPeriod))); + } // If a record path is specified if (_recordPath != nullptr && std::strlen(_recordPath) > 0) diff --git a/src/ign.hh b/src/ign.hh index d16add288e..38de2a88e6 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -57,6 +57,7 @@ extern "C" const char *worldInstallDir(); /// it receives a world path from GUI. /// null to record the default topics. /// \param[in] _headless True if server rendering should run headless +/// \param[in] _recordPeriod --record-period option /// \return 0 if successful, 1 if not. extern "C" int runServer(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, @@ -65,7 +66,8 @@ extern "C" int runServer(const char *_sdfString, int _logCompress, const char *_playback, const char *_physicsEngine, const char *_renderEngineServer, const char *_renderEngineGui, const char *_file, - const char *_recordTopics, int _waitGui, int _headless); + const char *_recordTopics, int _waitGui, int _headless, + float _recordPeriod); /// \brief External hook to run simulation GUI. /// \param[in] _guiConfig Path to Ignition GUI configuration file. diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index 87995ca0d0..918359f6dc 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -26,6 +26,7 @@ #include #include +#include #include #include #include @@ -157,6 +158,12 @@ class ignition::gazebo::systems::LogRecordPrivate /// \brief List of saved models if record with resources is enabled. public: std::set savedModels; + + /// \brief Time period between state recording + public: std::chrono::steady_clock::duration recordPeriod{0}; + + /// \brief Last time states are recorded + public: std::chrono::steady_clock::duration lastRecordSimTime{0}; }; bool LogRecordPrivate::started{false}; @@ -209,6 +216,11 @@ void LogRecord::Configure(const Entity &_entity, this->dataPtr->SetRecordResources(_sdf->Get("record_resources", false).first); + this->dataPtr->recordPeriod = + std::chrono::duration_cast( + std::chrono::duration( + _sdf->Get("record_period", 0.0).first)); + this->dataPtr->compress = _sdf->Get("compress", false).first; this->dataPtr->cmpPath = _sdf->Get("compress_path", "").first; @@ -690,16 +702,34 @@ void LogRecord::PostUpdate(const UpdateInfo &_info, } } + bool record = true; + if (this->dataPtr->recordPeriod > std::chrono::steady_clock::duration::zero()) + { + if (_ecm.HasOneTimeComponentChanges() || + (_info.simTime - this->dataPtr->lastRecordSimTime) >= + this->dataPtr->recordPeriod) + { + this->dataPtr->lastRecordSimTime = _info.simTime; + } + else + { + record = false; + } + } + // TODO(louise) Use the SceneBroadcaster's topic once that publishes // the changed state // \todo(anyone) A potential enhancement here is have a keyframe mechanism // to store complete state periodically, and then store incremental from // that. It would reduce some of the compute on replaying // (especially in tools like plotting or seeking through logs). - msgs::SerializedStateMap stateMsg; - _ecm.ChangedState(stateMsg); - if (!stateMsg.entities().empty()) - this->dataPtr->statePub.Publish(stateMsg); + if (record) + { + msgs::SerializedStateMap stateMsg; + _ecm.ChangedState(stateMsg); + if (!stateMsg.entities().empty()) + this->dataPtr->statePub.Publish(stateMsg); + } // If there are new models loaded, save meshes and textures if (this->dataPtr->RecordResources() && _ecm.HasNewEntities()) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index a8c5d2d857..f88ae5b2bd 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -45,6 +45,7 @@ #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/LogicalCamera.hh" +#include "ignition/gazebo/components/LogPlaybackStatistics.hh" #include "ignition/gazebo/components/Material.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -252,6 +253,10 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate /// \brief Store SDF scene information so that it can be inserted into /// scene message. public: sdf::Scene sdfScene; + + /// \brief Flag used to indicate if periodic changes need to be published + /// This is currently only used in playback mode. + public: bool pubPeriodicChanges{false}; }; ////////////////////////////////////////////////// @@ -351,8 +356,11 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, auto now = std::chrono::system_clock::now(); bool itsPubTime = (now - this->dataPtr->lastStatePubTime > this->dataPtr->statePublishPeriod[_info.paused]); + // check if we need to publish periodic changes in playback mode. + bool pubChanges = this->dataPtr->pubPeriodicChanges && + _manager.HasPeriodicComponentChanges(); auto shouldPublish = this->dataPtr->statePub.HasConnections() && - (changeEvent || itsPubTime); + (changeEvent || itsPubTime || pubChanges); if (this->dataPtr->stateServiceRequest || shouldPublish) { @@ -375,9 +383,39 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, else if (!_info.paused) { IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState"); - auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges(); - _manager.State(*this->dataPtr->stepMsg.mutable_state(), - {}, periodicComponents); + + if (_manager.HasPeriodicComponentChanges()) + { + auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges(); + _manager.State(*this->dataPtr->stepMsg.mutable_state(), + {}, periodicComponents); + this->dataPtr->pubPeriodicChanges = false; + } + else + { + // log files may be recorded at lower rate than sim time step. So in + // playback mode, the scene broadcaster may not see any periodic + // changed states here since it no longer happens every iteration. + // As the result, no state changes are published to be GUI, causing + // visuals in the GUI scene to miss updates. The visuals are only + // updated if by some timing coincidence that log playback updates + // the ECM at the same iteration as when the scene broadcaster is going + // to publish perioidc changes here. + // To work around the issue, we force the scene broadcaster + // to publish states at an offcycle iteration the next time it sees + // periodic changes. + auto playbackComp = + _manager.Component( + this->dataPtr->worldEntity); + if (playbackComp) + { + this->dataPtr->pubPeriodicChanges = true; + } + // this creates an empty state in the msg even there are no periodic + // changed components - done to preseve existing behavior. + // we may be able to remove this in the future and update tests + this->dataPtr->stepMsg.mutable_state(); + } } // Full state on demand diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 233ae638fa..9177378dc0 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -1643,3 +1643,84 @@ TEST_F(LogSystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(LogTopics)) this->CreateLogsDir(); #endif } + +///////////////////////////////////////////////// +TEST_F(LogSystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RecordPeriod)) +{ + // Create temp directory to store log + this->CreateLogsDir(); + + // test world + const auto recordSdfPath = common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "test", "worlds", + "log_record_resources.sdf"); + + // Change environment variable so that downloaded fuel files aren't written + // to $HOME + std::string homeOrig; + common::env(IGN_HOMEDIR, homeOrig); + std::string homeFake = common::joinPaths(this->logsDir, "default"); + EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); + + const std::string recordPath = this->logDir; + std::string statePath = common::joinPaths(recordPath, "state.tlog"); + + int numIterations = 100; +#ifndef __APPLE__ + // Log from command line + { + // Command line triggers ign.cc, which handles initializing ignLogDirectory + std::string cmd = kIgnCommand + " -r -v 4 --iterations " + + std::to_string(numIterations) + " " + + "--record-period 0.002 " + + "--record-path " + recordPath + " " + recordSdfPath; + std::cout << "Running command [" << cmd << "]" << std::endl; + + // Run + std::string output = customExecStr(cmd); + std::cout << output << std::endl; + } + + std::string consolePath = common::joinPaths(recordPath, "server_console.log"); + EXPECT_TRUE(common::exists(consolePath)) << consolePath; + EXPECT_TRUE(common::exists(statePath)) << statePath; + + // Recorded models should exist + EXPECT_GT(entryCount(recordPath), 1); + + // Verify file is created + auto logFile = common::joinPaths(recordPath, "state.tlog"); + EXPECT_TRUE(common::exists(logFile)); + + // Load the state log file into a player. + transport::log::Playback player(statePath); + const int64_t addTopicResult = player.AddTopic(std::regex(".*")); + + // There should be 2 topics (sdf, & state) + EXPECT_EQ(2, addTopicResult); + + int msgCount = 0; + std::function stateCb = + [&](const msgs::SerializedStateMap &) -> void + { + msgCount++; + }; + + // Subscribe to the state topic + transport::Node node; + node.Subscribe("/world/default/changed_state", stateCb); + + // Begin playback + transport::log::PlaybackHandlePtr handle = + player.Start(std::chrono::seconds(5), false); + handle->WaitUntilFinished(); + + // There were 100 iterations of simulation, and we were recording at 2ms + // so there should be 50 state messages. + EXPECT_EQ(50, msgCount); + + // Remove artifacts. Recreate new directory + this->RemoveLogsDir(); + this->CreateLogsDir(); +#endif +} From 594955d051625e73ca29c66feab797c4f1e78087 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 17 Aug 2022 11:14:40 -0700 Subject: [PATCH 07/13] 6.11.0 (#1656) * prep for 6.11.0 release Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 79 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 020891d81d..5efa74bd84 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.10.0) +project(ignition-gazebo6 VERSION 6.11.0) set (GZ_DISTRIBUTION "Fortress") #============================================================================ diff --git a/Changelog.md b/Changelog.md index 325bb7338d..d9bb90509e 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,83 @@ ## Ignition Gazebo 6.x +### Gazebo Sim 6.11.0 (2022-08-17) + +1. Add support for specifying log record period + * [Pull request #1636](https://github.com/gazebosim/gz-sim/pull/1636) + +1. Common widget GzColor replacement + * [Pull request #1530](https://github.com/gazebosim/gz-sim/pull/1530) + +1. Replace plotIcon in ComponentInspector with GzPlotIcon + * [Pull request #1638](https://github.com/gazebosim/gz-sim/pull/1638) + +1. Component Inspector with common widget pose plotting + * [Pull request #1607](https://github.com/gazebosim/gz-sim/pull/1607) + +1. Change CODEOWNERS and maintainer to Michael + * [Pull request #1644](https://github.com/gazebosim/gz-sim/pull/1644) + +1. Replace pose in ViewAngle with GzPose + * [Pull request #1641](https://github.com/gazebosim/gz-sim/pull/1641) + +1. Add system to an entity through Component Inspector + * [Pull request #1549](https://github.com/gazebosim/gz-sim/pull/1549) + +1. Quick start dialog + * [Pull request #1536](https://github.com/gazebosim/gz-sim/pull/1536) + * [Pull request #1627](https://github.com/gazebosim/gz-sim/pull/1627) + +1. Quiet libSDFormat console on --verbose 0 + * [Pull request #1621](https://github.com/gazebosim/gz-sim/pull/1621) + +1. New Apply Link Wrench system + * [Pull request #1593](https://github.com/gazebosim/gz-sim/pull/1593) + +1. Add Tf publishing to AckermannSteering system + * [Pull request #1576](https://github.com/gazebosim/gz-sim/pull/1576) + +1. Fix component updates + * [Pull request #1580](https://github.com/gazebosim/gz-sim/pull/1580) + +1. Implement vector3 with common widget vector3 + * [Pull request #1569](https://github.com/gazebosim/gz-sim/pull/1569) + +1. Fix to modelphotoshoot test + * [Pull request #1570](https://github.com/gazebosim/gz-sim/pull/1570) + +1. Update log playback gui config + * [Pull request #1590](https://github.com/gazebosim/gz-sim/pull/1590) + +1. Helper function to get an entity from an entity message + * [Pull request #1595](https://github.com/gazebosim/gz-sim/pull/1595) + +1. Fix compilation of scene broadcaster test + * [Pull request #1599](https://github.com/gazebosim/gz-sim/pull/1599) + +1. Ignition -> Gazebo + * [Pull request #1596](https://github.com/gazebosim/gz-sim/pull/1596) + +1. Add Model::CanonicalLink getter + * [Pull request #1594](https://github.com/gazebosim/gz-sim/pull/1594) + +1. Implement Pose3d with common widget pose + * [Pull request #1571](https://github.com/gazebosim/gz-sim/pull/1571) + +1. Fix UNIT_Server_TEST on Windows + * [Pull request #1577](https://github.com/gazebosim/gz-sim/pull/1577) + +1. Use pytest to generate junit xml files for python tests + * [Pull request #1562](https://github.com/gazebosim/gz-sim/pull/1562) + +1. Refactor: Utilizes function to load animations + * [Pull request #1568](https://github.com/gazebosim/gz-sim/pull/1568) + +1. Utilizes function to sequence trajectories + * [Pull request #1565](https://github.com/gazebosim/gz-sim/pull/1565) + +1. Disable MacOS flakies Citadel + * [Pull request #1545](https://github.com/gazebosim/gz-sim/pull/1545) + ### Gazebo Sim 6.10.0 (2022-06-24) 1. Expose the ability to stop a server from C++ From 4f93cde826be0b1298f4eab67aae15bd99ffb2ec Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 17 Aug 2022 14:37:43 -0700 Subject: [PATCH 08/13] Fix Windows and Doxygen (#1643) Signed-off-by: Louise Poubel --- src/LevelManager.cc | 6 +++--- src/gui/QuickStartHandler.hh | 8 ++++---- src/systems/elevator/Elevator.hh | 3 +-- src/systems/elevator/ElevatorCommonPrivate.hh | 3 +-- src/systems/elevator/ElevatorStateMachine.hh | 3 +-- src/systems/elevator/state_machine/ActionsImpl.hh | 3 +-- .../elevator/state_machine/ElevatorStateMachineImpl.hh | 3 +-- src/systems/elevator/state_machine/EventsImpl.hh | 3 +-- src/systems/elevator/state_machine/GuardsImpl.hh | 3 +-- src/systems/elevator/state_machine/StatesImpl.hh | 3 +-- src/systems/elevator/utils/DoorTimer.hh | 3 +-- src/systems/elevator/utils/JointMonitor.hh | 3 +-- .../optical_tactile_plugin/OpticalTactilePlugin.hh | 3 +-- src/systems/optical_tactile_plugin/Visualization.hh | 3 +-- src/systems/particle_emitter2/ParticleEmitter2.cc | 4 ++-- test/integration/apply_link_wrench_system.cc | 9 ++++++--- 16 files changed, 27 insertions(+), 36 deletions(-) diff --git a/src/LevelManager.cc b/src/LevelManager.cc index ffb53ad74a..fd3a1289d6 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -36,14 +36,14 @@ #include "ignition/gazebo/components/Geometry.hh" #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Level.hh" -#include "ignition/gazebo/components/Model.hh" -#include "ignition/gazebo/components/Light.hh" -#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/LevelBuffer.hh" #include "ignition/gazebo/components/LevelEntityNames.hh" +#include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/LinearVelocitySeed.hh" #include "ignition/gazebo/components/MagneticField.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerLevels.hh" diff --git a/src/gui/QuickStartHandler.hh b/src/gui/QuickStartHandler.hh index 8ac9c291e5..6b0d921eb2 100644 --- a/src/gui/QuickStartHandler.hh +++ b/src/gui/QuickStartHandler.hh @@ -20,8 +20,8 @@ #include #include -#include "ignition/gazebo/Export.hh" #include "ignition/gazebo/config.hh" +#include "ignition/gazebo/gui/Export.hh" namespace ignition { @@ -32,7 +32,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace gui { /// \brief Class for handling quick start dialog -class QuickStartHandler : public QObject +class IGNITION_GAZEBO_GUI_VISIBLE QuickStartHandler : public QObject { Q_OBJECT @@ -44,8 +44,8 @@ class QuickStartHandler : public QObject /// \return Distribution name, such as 'Citadel' public: Q_INVOKABLE QString Distribution() const; - /// \brief Get Gazebo version - /// \return gazebo version + /// \brief Get Gazebo Sim version + /// \return Version public: Q_INVOKABLE QString SimVersion() const; /// \brief Set starting world diff --git a/src/systems/elevator/Elevator.hh b/src/systems/elevator/Elevator.hh index cbb44da5b7..f5506ba5f2 100644 --- a/src/systems/elevator/Elevator.hh +++ b/src/systems/elevator/Elevator.hh @@ -32,8 +32,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Data forward declaration diff --git a/src/systems/elevator/ElevatorCommonPrivate.hh b/src/systems/elevator/ElevatorCommonPrivate.hh index ec787e3b91..ee73976e81 100644 --- a/src/systems/elevator/ElevatorCommonPrivate.hh +++ b/src/systems/elevator/ElevatorCommonPrivate.hh @@ -34,8 +34,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { class ElevatorCommonPrivate diff --git a/src/systems/elevator/ElevatorStateMachine.hh b/src/systems/elevator/ElevatorStateMachine.hh index 6dfa7811f3..ef84d5faaa 100644 --- a/src/systems/elevator/ElevatorStateMachine.hh +++ b/src/systems/elevator/ElevatorStateMachine.hh @@ -35,8 +35,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Data forward declarations diff --git a/src/systems/elevator/state_machine/ActionsImpl.hh b/src/systems/elevator/state_machine/ActionsImpl.hh index e613fc6388..9aff496721 100644 --- a/src/systems/elevator/state_machine/ActionsImpl.hh +++ b/src/systems/elevator/state_machine/ActionsImpl.hh @@ -27,8 +27,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { namespace actions diff --git a/src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh b/src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh index a995a69ecd..d1260e3cab 100644 --- a/src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh +++ b/src/systems/elevator/state_machine/ElevatorStateMachineImpl.hh @@ -36,8 +36,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { class ElevatorStateMachinePrivate diff --git a/src/systems/elevator/state_machine/EventsImpl.hh b/src/systems/elevator/state_machine/EventsImpl.hh index 1aa4d9a781..8191eefbeb 100644 --- a/src/systems/elevator/state_machine/EventsImpl.hh +++ b/src/systems/elevator/state_machine/EventsImpl.hh @@ -25,8 +25,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { namespace events diff --git a/src/systems/elevator/state_machine/GuardsImpl.hh b/src/systems/elevator/state_machine/GuardsImpl.hh index ec5ac4c706..b781ecd523 100644 --- a/src/systems/elevator/state_machine/GuardsImpl.hh +++ b/src/systems/elevator/state_machine/GuardsImpl.hh @@ -27,8 +27,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { namespace guards diff --git a/src/systems/elevator/state_machine/StatesImpl.hh b/src/systems/elevator/state_machine/StatesImpl.hh index accd7f4a47..6885c436f6 100644 --- a/src/systems/elevator/state_machine/StatesImpl.hh +++ b/src/systems/elevator/state_machine/StatesImpl.hh @@ -32,8 +32,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { /// \brief State at which the elevator is idling. diff --git a/src/systems/elevator/utils/DoorTimer.hh b/src/systems/elevator/utils/DoorTimer.hh index bcfeb18a2f..8b5aa0cc8a 100644 --- a/src/systems/elevator/utils/DoorTimer.hh +++ b/src/systems/elevator/utils/DoorTimer.hh @@ -34,8 +34,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Data forward declaration diff --git a/src/systems/elevator/utils/JointMonitor.hh b/src/systems/elevator/utils/JointMonitor.hh index a42046b4ab..f472ec1d8f 100644 --- a/src/systems/elevator/utils/JointMonitor.hh +++ b/src/systems/elevator/utils/JointMonitor.hh @@ -33,8 +33,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Data forward declaration diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 687246b6a8..30853d164a 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -27,8 +27,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { // Forward declaration diff --git a/src/systems/optical_tactile_plugin/Visualization.hh b/src/systems/optical_tactile_plugin/Visualization.hh index 978b3e5aca..a5e863e139 100644 --- a/src/systems/optical_tactile_plugin/Visualization.hh +++ b/src/systems/optical_tactile_plugin/Visualization.hh @@ -32,8 +32,7 @@ namespace ignition namespace gazebo { // Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE -{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { namespace optical_tactile_sensor diff --git a/src/systems/particle_emitter2/ParticleEmitter2.cc b/src/systems/particle_emitter2/ParticleEmitter2.cc index b8374f3a55..6e0c290992 100644 --- a/src/systems/particle_emitter2/ParticleEmitter2.cc +++ b/src/systems/particle_emitter2/ParticleEmitter2.cc @@ -158,7 +158,7 @@ void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info, // hack to avoid breaking the particle_emitter.proto message. if (_emitter->Data().has_header()) { - for (const auto data : _emitter->Data().header().data()) + for (const auto &data : _emitter->Data().header().data()) { if (data.key() == "topic" && !data.value().empty()) { @@ -218,7 +218,7 @@ void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info, return; // Process each command - for (const auto cmd : this->dataPtr->userCmd) + for (const auto &cmd : this->dataPtr->userCmd) { // Create component. auto emitterComp = _ecm.Component( diff --git a/test/integration/apply_link_wrench_system.cc b/test/integration/apply_link_wrench_system.cc index b937bd87a8..9da7c6777a 100644 --- a/test/integration/apply_link_wrench_system.cc +++ b/test/integration/apply_link_wrench_system.cc @@ -23,6 +23,7 @@ #include #include #include +#include #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" @@ -48,7 +49,7 @@ class ApplyLinkWrenchTestFixture : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(ApplyLinkWrenchTestFixture, FromSdf) +TEST_F(ApplyLinkWrenchTestFixture, IGN_UTILS_TEST_DISABLED_ON_WIN32(FromSdf)) { TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "apply_link_wrench.sdf")); @@ -106,7 +107,8 @@ TEST_F(ApplyLinkWrenchTestFixture, FromSdf) } ///////////////////////////////////////////////// -TEST_F(ApplyLinkWrenchTestFixture, PersistentFromTopic) +TEST_F(ApplyLinkWrenchTestFixture, + IGN_UTILS_TEST_DISABLED_ON_WIN32(PersistentFromTopic)) { TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "apply_link_wrench.sdf")); @@ -240,7 +242,8 @@ TEST_F(ApplyLinkWrenchTestFixture, PersistentFromTopic) } ///////////////////////////////////////////////// -TEST_F(ApplyLinkWrenchTestFixture, InstantaneousFromTopic) +TEST_F(ApplyLinkWrenchTestFixture, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InstantaneousFromTopic)) { TestFixture fixture(common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "apply_link_wrench.sdf")); From 25bdef685919196be79af5083710152dcff4871d Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Wed, 24 Aug 2022 01:02:02 -0700 Subject: [PATCH 09/13] Adding tests for hydrodynamics (#1617) * ball drop test Signed-off-by: Dharini Dutia * applied feedback Signed-off-by: Dharini Dutia * added world force Signed-off-by: Dharini Dutia * typo Signed-off-by: Dharini Dutia * updating upstream from lrauv Signed-off-by: Dharini Dutia * fixing mass, inertia and drag coeff Signed-off-by: Dharini Dutia * velocity and transform tests with multiple models Signed-off-by: Dharini Dutia * fix cpplint Signed-off-by: Dharini Dutia * feedback Signed-off-by: Dharini Dutia * reverting to old stateDot Signed-off-by: Dharini Dutia Signed-off-by: Dharini Dutia --- src/systems/hydrodynamics/Hydrodynamics.cc | 6 +- test/integration/CMakeLists.txt | 1 + test/integration/hydrodynamics.cc | 170 ++++++++++++ test/worlds/hydrodynamics.sdf | 307 +++++++++++++++++++++ 4 files changed, 482 insertions(+), 2 deletions(-) create mode 100644 test/integration/hydrodynamics.cc create mode 100644 test/worlds/hydrodynamics.sdf diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 5d278790f8..ac7f8fdf3d 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -296,7 +296,7 @@ void Hydrodynamics::PreUpdate( // These variables follow Fossen's scheme in "Guidance and Control // of Ocean Vehicles." The `state` vector contains the ship's current velocity - // in the formate [x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel]. + // in the format [x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel]. // `stateDot` consists of the first derivative in time of the state vector. // `Cmat` corresponds to the Centripetal matrix // `Dmat` is the drag matrix @@ -340,13 +340,15 @@ void Hydrodynamics::PreUpdate( state(4) = localRotationalVelocity.Y(); state(5) = localRotationalVelocity.Z(); + // TODO(anyone) Make this configurable auto dt = static_cast(_info.dt.count())/1e9; stateDot = (state - this->dataPtr->prevState)/dt; this->dataPtr->prevState = state; // The added mass - const Eigen::VectorXd kAmassVec = this->dataPtr->Ma * stateDot; + // Negative sign signifies the behaviour change + const Eigen::VectorXd kAmassVec = - this->dataPtr->Ma * stateDot; // Coriolis and Centripetal forces for under water vehicles (Fossen P. 37) // Note: this is significantly different from VRX because we need to account diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 9b79fe53ce..ad026ed661 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -24,6 +24,7 @@ set(tests force_torque_system.cc fuel_cached_server.cc halt_motion.cc + hydrodynamics.cc imu_system.cc joint_controller_system.cc joint_position_controller_system.cc diff --git a/test/integration/hydrodynamics.cc b/test/integration/hydrodynamics.cc new file mode 100644 index 0000000000..cfcab01f93 --- /dev/null +++ b/test/integration/hydrodynamics.cc @@ -0,0 +1,170 @@ +/* + * Copyright (C) 2022 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 "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/World.hh" + +#include "ignition/gazebo/test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class HydrodynamicsTest : public InternalFixture<::testing::Test> +{ + /// \brief Test a world file + /// \param[in] _world Path to world file + /// \param[in] _namespace Namespace for topic + /// \param[in] _density Fluid density + /// \param[in] _viscosity Fluid viscosity + /// \param[in] _radius Body's radius + /// \param[in] _area Body surface area + /// \param[in] _drag_coeff Body drag coefficient + public: std::vector TestWorld(const std::string &_world, + const std::string &_namespace); +}; + +////////////////////////////////////////////////// +std::vector HydrodynamicsTest::TestWorld( + const std::string &_world, const std::string &_namespace) +{ + // Maximum verbosity for debugging + ignition::common::Console::SetVerbosity(4); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link body; + std::vector bodyVels; + fixture. + OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, _namespace); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, _namespace + "_link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + // Add force + math::Vector3d force(0, 0, 10.0); + body.AddWorldForce(_ecm, force); + }). + OnPostUpdate([&](const UpdateInfo &/*_info*/, + const EntityComponentManager &_ecm) + { + auto bodyVel = body.WorldLinearVelocity(_ecm); + ASSERT_TRUE(bodyVel); + bodyVels.push_back(bodyVel.value()); + }). + Finalize(); + + fixture.Server()->Run(true, 1000, false); + EXPECT_EQ(1000u, bodyVels.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(body.Entity(), kNullEntity); + + return bodyVels; +} + +///////////////////////////////////////////////// +/// This test evaluates whether the hydrodynamic plugin affects the motion +/// of the body when a force is applied. +TEST_F(HydrodynamicsTest, VelocityTestinOil) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "hydrodynamics.sdf"); + + auto sphere1Vels = this->TestWorld(world, "sphere1"); + auto sphere2Vels = this->TestWorld(world, "sphere2"); + + for (unsigned int i = 0; i < 1000; ++i) + { + // Sanity check + EXPECT_FLOAT_EQ(0.0, sphere1Vels[i].X()); + EXPECT_FLOAT_EQ(0.0, sphere1Vels[i].Y()); + EXPECT_FLOAT_EQ(0.0, sphere2Vels[i].X()); + EXPECT_FLOAT_EQ(0.0, sphere2Vels[i].Y()); + + // Wait a couple of iterations for the body to move + if(i > 4) + { + EXPECT_LT(sphere1Vels[i].Z(), sphere2Vels[i].Z()); + + if (i > 900) + { + // Expect for the velocity to stabilize + EXPECT_NEAR(sphere1Vels[i-1].Z(), sphere1Vels[i].Z(), 1e-6); + EXPECT_NEAR(sphere2Vels[i-1].Z(), sphere2Vels[i].Z(), 1e-6); + } + } + } +} + +///////////////////////////////////////////////// +/// This test makes sure that the transforms of the hydrodynamics +/// plugin are correct by comparing 3 cylinders in different +/// positions and orientations. +TEST_F(HydrodynamicsTest, TransformsTestinWater) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "hydrodynamics.sdf"); + + auto cylinder1Vels = this->TestWorld(world, "cylinder1"); + auto cylinder2Vels = this->TestWorld(world, "cylinder2"); + auto cylinder3Vels = this->TestWorld(world, "cylinder3"); + + for (unsigned int i = 900; i < 1000; ++i) + { + // Expect for the velocity to stabilize + EXPECT_NEAR(cylinder1Vels[i-1].Z(), cylinder1Vels[i].Z(), 1e-6); + EXPECT_NEAR(cylinder2Vels[i-1].Z(), cylinder2Vels[i].Z(), 1e-6); + EXPECT_NEAR(cylinder3Vels[i-1].Z(), cylinder3Vels[i].Z(), 1e-6); + + // Expect for final velocities to be similar + EXPECT_NEAR(cylinder1Vels[i].Z(), cylinder2Vels[i].Z(), 1e-4); + EXPECT_NEAR(cylinder2Vels[i].Z(), cylinder3Vels[i].Z(), 1e-4); + } +} diff --git a/test/worlds/hydrodynamics.sdf b/test/worlds/hydrodynamics.sdf new file mode 100644 index 0000000000..234d6314ea --- /dev/null +++ b/test/worlds/hydrodynamics.sdf @@ -0,0 +1,307 @@ + + + + + + 0.001 + + 0 + + + + 0 0 0 + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 1 0.25 0 0 0 0 + + 25 + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + + + 1 -0.25 0 0 0 0 + + 25 + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + sphere2_link + 918 + 0 + 0 + 15.381 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 11.5359 + 0.211869 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + + 40 + + 0.8 + 0 + 0 + 0.8 + 0 + 0.8 + + + + + + + 0.2 + 1 + + + + + + + 0.2 + 0.1 + + + + + + + cylinder1_link + 1000 + 0 + 0 + 12.5664 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 94.2475 + 0.0037699 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + -1 0.5 0 45 0 45 + + 40 + + 0.8 + 0 + 0 + 0.8 + 0 + 0.8 + + + + + + + 0.2 + 1 + + + + + + + 0.2 + 0.1 + + + + + + + cylinder2_link + 1000 + 0 + 0 + 12.5664 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 94.2475 + 0.0037699 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + -1 -0.5 0 0 90 0 + + 40 + + 0.8 + 0 + 0 + 0.8 + 0 + 0.8 + + + + + + + 0.2 + 1 + + + + + + + 0.2 + 0.1 + + + + + + + cylinder3_link + 1000 + 0 + 0 + 12.5664 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 94.2475 + 0.0037699 + 0 + 0 + 0 + 0 + 0 + 0 + + + + From 85bab4c99ab7f72bec5af92e1aec995b75fa0ac5 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 29 Aug 2022 07:41:55 +0100 Subject: [PATCH 10/13] Add information about system paramter (#1671) Adding some API docs for changes from https://github.com/gazebosim/gz-sim/pull/1076 Signed-off-by: Kenji Brameld --- src/systems/joint_state_publisher/JointStatePublisher.hh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index 75c13f15a7..4ea973ddd9 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -35,7 +35,7 @@ namespace systems { /// \brief The JointStatePub system publishes state information for /// a model. The published message type is ignition::msgs::Model, and the - /// publication topic is "/world//model//state". + /// publication topic is determined by the `` parameter. /// /// By default the JointStatePublisher will publish all joints for /// a model. Use the `` system parameter, described below, to @@ -43,6 +43,9 @@ namespace systems /// /// # System Parameters /// + /// ``: Name of the topic to publish to. This parameter is optional, + /// and if not provided, the joint state will be published to + /// "/world//model//state". /// ``: Name of a joint to publish. This parameter can be /// specified multiple times, and is optional. All joints in a model will /// be published if joint names are not specified. From cd91d96ffa02d5de498fab0e1fc9261f80c95ae7 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 6 Oct 2022 11:06:32 -0700 Subject: [PATCH 11/13] fix build Signed-off-by: Nate Koenig --- test/integration/log_system.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 94da71a941..d1b5bdf86b 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -1645,7 +1645,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogTopics)) } ///////////////////////////////////////////////// -TEST_F(LogSystemTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RecordPeriod)) +TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RecordPeriod)) { // Create temp directory to store log this->CreateLogsDir(); From dd32beff546aefc9e0bd211c6af75929c5400d24 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 6 Oct 2022 11:23:05 -0700 Subject: [PATCH 12/13] fix build, really Signed-off-by: Nate Koenig --- test/integration/hydrodynamics.cc | 38 +++++++++++++++---------------- test/integration/log_system.cc | 12 +++++----- 2 files changed, 25 insertions(+), 25 deletions(-) diff --git a/test/integration/hydrodynamics.cc b/test/integration/hydrodynamics.cc index cfcab01f93..97627b2e9f 100644 --- a/test/integration/hydrodynamics.cc +++ b/test/integration/hydrodynamics.cc @@ -17,26 +17,26 @@ #include -#include - -#include -#include -#include -#include - -#include "ignition/gazebo/Link.hh" -#include "ignition/gazebo/Model.hh" -#include "ignition/gazebo/Server.hh" -#include "ignition/gazebo/SystemLoader.hh" -#include "ignition/gazebo/TestFixture.hh" -#include "ignition/gazebo/Util.hh" -#include "ignition/gazebo/World.hh" - -#include "ignition/gazebo/test_config.hh" +#include + +#include +#include +#include +#include + +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/TestFixture.hh" +#include "gz/sim/Util.hh" +#include "gz/sim/World.hh" + +#include "test_config.hh" #include "../helpers/EnvTestFixture.hh" -using namespace ignition; -using namespace gazebo; +using namespace gz; +using namespace sim; class HydrodynamicsTest : public InternalFixture<::testing::Test> { @@ -57,7 +57,7 @@ std::vector HydrodynamicsTest::TestWorld( const std::string &_world, const std::string &_namespace) { // Maximum verbosity for debugging - ignition::common::Console::SetVerbosity(4); + common::Console::SetVerbosity(4); // Start server ServerConfig serverConfig; diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index d1b5bdf86b..ef91066f2c 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -1523,7 +1523,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogResources)) // Recorded models should exist EXPECT_GT(entryCount(recordPath), 2); EXPECT_TRUE(common::exists(common::joinPaths(recordPath, homeFake, - ".gz", "fuel", "fuel.ignitionrobotics.org", "openrobotics", + ".gz", "fuel", "fuel.gazebosim.org", "openrobotics", "models", "x2 config 1"))); // Remove artifacts. Recreate new directory @@ -1558,11 +1558,11 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LogResources)) EXPECT_GT(entryCount(recordPath), 1); #endif EXPECT_TRUE(common::exists(common::joinPaths(recordPath, homeFake, - ".gz", "fuel", "fuel.ignitionrobotics.org", "openrobotics", + ".gz", "fuel", "fuel.gazebosim.org", "openrobotics", "models", "x2 config 1"))); // Revert environment variable after test is done - EXPECT_TRUE(gz::common::setenv(GZ_HOMEDIR, homeOrig.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeOrig.c_str())); // Remove artifacts this->RemoveLogsDir(); @@ -1658,9 +1658,9 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RecordPeriod)) // Change environment variable so that downloaded fuel files aren't written // to $HOME std::string homeOrig; - common::env(IGN_HOMEDIR, homeOrig); + common::env(GZ_HOMEDIR, homeOrig); std::string homeFake = common::joinPaths(this->logsDir, "default"); - EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); + EXPECT_TRUE(common::setenv(GZ_HOMEDIR, homeFake.c_str())); const std::string recordPath = this->logDir; std::string statePath = common::joinPaths(recordPath, "state.tlog"); @@ -1670,7 +1670,7 @@ TEST_F(LogSystemTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RecordPeriod)) // Log from command line { // Command line triggers ign.cc, which handles initializing ignLogDirectory - std::string cmd = kIgnCommand + " -r -v 4 --iterations " + std::string cmd = kGzCommand + " -r -v 4 --iterations " + std::to_string(numIterations) + " " + "--record-period 0.002 " + "--record-path " + recordPath + " " + recordSdfPath; From 6a8594b59feb7766ea4257754bbdcf71c147c881 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Thu, 6 Oct 2022 11:38:01 -0700 Subject: [PATCH 13/13] fix tests Signed-off-by: Nate Koenig --- test/integration/ackermann_steering_system.cc | 3 ++- test/worlds/log_record_resources.sdf | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index b05d8647c2..886cc43c3d 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -414,7 +414,8 @@ TEST_P(AckermannSteeringTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(TfPublishes)) int sleep = 0; int maxSleep = 30; - for (; odomPoses.size() < 3 && sleep < maxSleep; ++sleep) + for (; (odomPoses.size() < 3 || odomPoses.size() != tfPoses.size()) && + sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } diff --git a/test/worlds/log_record_resources.sdf b/test/worlds/log_record_resources.sdf index c95e80ca59..6a75af7983 100644 --- a/test/worlds/log_record_resources.sdf +++ b/test/worlds/log_record_resources.sdf @@ -53,7 +53,7 @@ false staging_area 0 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/X2 Config 1 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/X2 Config 1