From 4b643d22c9fdcae0ad9c4fddd0fda15d7210002d Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 3 Jun 2022 08:48:53 -0700 Subject: [PATCH 01/13] =?UTF-8?q?=F0=9F=8E=88=203.13.0=20(#1512)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- Changelog.md | 57 +++++++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 57 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 550e666c33..79e93df766 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-gazebo3 VERSION 3.12.0) +project(ignition-gazebo3 VERSION 3.13.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 515b30c4cd..0d62e41cce 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,61 @@ ## Ignition Gazebo 3.x -### Ignition Gazebo 3.X.X (20XX-XX-XX) +### Ignition Gazebo 3.13.0 (2022-06-01) + +1. Extruded 2D polyline geometries + * [Pull request #1456](https://github.com/gazebosim/gz-sim/pull/1456) + +1. Add elevator system + * [Pull request #535](https://github.com/gazebosim/gz-sim/pull/535) + +1. Add desktop entry and svg logo + * [Pull request #1411](https://github.com/gazebosim/gz-sim/pull/1411) + * [Pull request #1430](https://github.com/gazebosim/gz-sim/pull/1430) + +1. Delete unused `gazebo.hh.in` + * [Pull request #1490](https://github.com/gazebosim/gz-sim/pull/1490) + +1. Add repo specific issue templates + * [Pull request #1461](https://github.com/gazebosim/gz-sim/pull/1461) + +1. Added user command to set multiple entities' poses + * [Pull request #1394](https://github.com/gazebosim/gz-sim/pull/1394) + +1. Component inspector: refactor Pose3d C++ code into a separate class + * [Pull request #1400](https://github.com/gazebosim/gz-sim/pull/1400) + +1. Added more sensor properties to `scene/info` topic + * [Pull request #1344](https://github.com/gazebosim/gz-sim/pull/1344) + +1. `JointStatePublisher` publish parent, child and axis data + * [Pull request #1345](https://github.com/gazebosim/gz-sim/pull/1345) + +1. Removed unused variables in shapes plugin + * [Pull request #1321](https://github.com/gazebosim/gz-sim/pull/1321) + +1. Log an error if `JointPositionController` cannot find the joint. (citadel retarget) + * [Pull request #1314](https://github.com/gazebosim/gz-sim/pull/1314) + +1. `Buoyancy`: fix center of volume's reference frame + * [Pull request #1302](https://github.com/gazebosim/gz-sim/pull/1302) + +1. Remove `EachNew` calls from sensor PreUpdates + * [Pull request #1281](https://github.com/gazebosim/gz-sim/pull/1281) + +1. Prevent `GzScene3D` 💥 if another scene is already loaded + * [Pull request #1294](https://github.com/gazebosim/gz-sim/pull/1294) + +1. Add `project()` call to examples + * [Pull request #1274](https://github.com/gazebosim/gz-sim/pull/1274) + +1. Implement `/server_control::stop` + * [Pull request #1240](https://github.com/gazebosim/gz-sim/pull/1240) + +1. 👩‍🌾 Make depth camera tests more robust + * [Pull request1257](https://github.com/gazebosim/gz-sim/pull/1257) + +1. Make tests run as fast as possible + * [Pull request #1194](https://github.com/gazebosim/gz-sim/pull/1194) ### Ignition Gazebo 3.12.0 (2021-11-11) From 5bb179f949edd29034e93e1abcef4d3c92ac84cd Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 6 Jun 2022 13:35:51 -0400 Subject: [PATCH 02/13] Add new GZ_GUI_RESOURCE_PATH to help message (#1470) Signed-off-by: Louise Poubel --- src/cmd/cmdgazebo.rb.in | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 30f783c6eb..2e9be3e21a 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -159,6 +159,8 @@ COMMANDS = { 'gazebo' => " IGN_GAZEBO_SERVER_CONFIG_PATH Path to server configuration file. \n\n"\ " IGN_GUI_PLUGIN_PATH Colon separated paths used to locate GUI \n"\ " plugins. \n\n"\ + " GZ_GUI_RESOURCE_PATH Colon separated paths used to locate GUI \n"\ + " resources such as configuration files. \n\n"\ " IGN_GAZEBO_NETWORK_ROLE Participant role used in a distributed \n"\ " simulation environment. Role is one of [PRIMARY, SECONDARY]. This is \n"\ " deprecated in ign-gazebo2. Please use --network-role instead. \n\n"\ From 900583a2203c770d3c7100255011cf328965d02e Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 9 Jun 2022 13:59:48 -0400 Subject: [PATCH 03/13] Fix faulty assumption in INTEGRATION_log_system (#1426) (#1531) Signed-off-by: Michael Carroll Co-authored-by: Michael Carroll --- test/integration/log_system.cc | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 3b81bf8929..09ca215c93 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -334,9 +334,8 @@ TEST_F(LogSystemTest, LogDefaults) EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); // Test case 1: - // No path specified, on both command line and SDF. This does not go through - // ign.cc, so ignLogDirectory() is not initialized (empty string). Recording - // should not take place. + // No path specified on command line. This does not go through + // ign.cc, recording should take place in the `.ignition` directory { // Change log path in SDF to empty sdf::Root recordSdfRoot; @@ -356,8 +355,12 @@ TEST_F(LogSystemTest, LogDefaults) recordServer.Run(true, 200, false); } - // Check ignLogDirectory is empty - EXPECT_TRUE(ignLogDirectory().empty()); + // We should expect to see "auto_default.log" and "state.tlog" + EXPECT_FALSE(ignLogDirectory().empty()); + EXPECT_TRUE(common::exists( + common::joinPaths(ignLogDirectory(), "auto_default.log"))); + EXPECT_TRUE(common::exists( + common::joinPaths(ignLogDirectory(), "state.tlog"))); // Remove artifacts. Recreate new directory this->RemoveLogsDir(); From b8c09b68765d84d00aa478c6e5e27ee1348f9517 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 13 Jun 2022 08:58:31 -0700 Subject: [PATCH 04/13] Fix sensors battery state test (#1529) Signed-off-by: Ian Chen --- test/integration/sensors_system_battery.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/sensors_system_battery.cc b/test/integration/sensors_system_battery.cc index 541ae9ac05..b7b65c0e11 100644 --- a/test/integration/sensors_system_battery.cc +++ b/test/integration/sensors_system_battery.cc @@ -192,8 +192,8 @@ TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(SensorsBatteryState)) // verify image count { std::lock_guard lock(mutex); - EXPECT_EQ(expectedImgCount, imgCount); - EXPECT_EQ(expectedImgCount, depthImgCount); + EXPECT_NEAR(expectedImgCount, imgCount, 1); + EXPECT_NEAR(expectedImgCount, depthImgCount, 1); imgCount = 0u; depthImgCount = 0u; } From dc5f117da91b19611bc244dd1ecbb04fc858140d Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Wed, 15 Jun 2022 13:59:34 -0400 Subject: [PATCH 05/13] Bash completion for flags (#1504) Signed-off-by: Mabel Zhang Signed-off-by: Louise Poubel Co-authored-by: Louise Poubel --- CMakeLists.txt | 3 ++ src/cmd/CMakeLists.txt | 22 +++++++++ src/cmd/gazebo.bash_completion.sh | 76 +++++++++++++++++++++++++++++++ src/cmd/model.bash_completion.sh | 55 ++++++++++++++++++++++ src/ign_TEST.cc | 66 +++++++++++++++++++++++++++ 5 files changed, 222 insertions(+) create mode 100644 src/cmd/gazebo.bash_completion.sh create mode 100644 src/cmd/model.bash_completion.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 79e93df766..4e2f9e21cc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,6 +136,9 @@ set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR}) ign_find_package(ignition-tools REQUIRED PKGCONFIG "ignition-tools") +# Note that CLI files are installed regardless of whether the dependency is +# available during build time +set(IGN_TOOLS_VER 1) #-------------------------------------- # Find protobuf diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index a1ff8091c4..11e7ef7e00 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -117,3 +117,25 @@ set(ign_model_ruby_path "${cmd_model_script_generated_test}") configure_file( "model.yaml.in" "${CMAKE_BINARY_DIR}/test/conf/model${PROJECT_VERSION_MAJOR}.yaml" @ONLY) + +#=============================================================================== +# Bash completion + +# Tack version onto and install the bash completion script +configure_file( + "gazebo.bash_completion.sh" + "${CMAKE_CURRENT_BINARY_DIR}/gazebo${PROJECT_VERSION_MAJOR}.bash_completion.sh" @ONLY) +install( + FILES + ${CMAKE_CURRENT_BINARY_DIR}/gazebo${PROJECT_VERSION_MAJOR}.bash_completion.sh + DESTINATION + ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${IGN_TOOLS_VER}.completion.d) + +configure_file( + "model.bash_completion.sh" + "${CMAKE_CURRENT_BINARY_DIR}/model${PROJECT_VERSION_MAJOR}.bash_completion.sh" @ONLY) +install( + FILES + ${CMAKE_CURRENT_BINARY_DIR}/model${PROJECT_VERSION_MAJOR}.bash_completion.sh + DESTINATION + ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/gz/gz${IGN_TOOLS_VER}.completion.d) diff --git a/src/cmd/gazebo.bash_completion.sh b/src/cmd/gazebo.bash_completion.sh new file mode 100644 index 0000000000..7d6f6d3042 --- /dev/null +++ b/src/cmd/gazebo.bash_completion.sh @@ -0,0 +1,76 @@ +#!/usr/bin/env bash +# +# 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. +# + +# bash tab-completion + +# This is a per-library function definition, used in conjunction with the +# top-level entry point in ign-tools. + +GZ_SIM_COMPLETION_LIST=" + -g + --iterations + --levels + --network-role + --network-secondaries + --record + --record-path + --record-resources + --record-topic + --log-overwrite + --log-compress + --playback + -r + -s + -v --verbose + --gui-config + --physics-engine + --render-engine + --render-engine-gui + --render-engine-server + --version + -z + -h --help + --force-version + --versions +" + +# TODO(anyone): In Garden+, replace `gazebo` in function name to align with +# subcommand, for ign-tools/etc/ign.bash_completion.sh to find this function. +function _gz_gazebo +{ + if [[ ${COMP_WORDS[COMP_CWORD]} == -* ]]; then + # Specify options (-*) word list for this subcommand + # TODO(anyone): In Fortress+, add --headless-rendering. + # Update ../ign_TEST.cc accordingly. + COMPREPLY=($(compgen -W "$GZ_SIM_COMPLETION_LIST" \ + -- "${COMP_WORDS[COMP_CWORD]}" )) + return + else + # Just use bash default auto-complete, because we never have two + # subcommands in the same line. If that is ever needed, change here to + # detect subsequent subcommands + COMPREPLY=($(compgen -o default -- "${COMP_WORDS[COMP_CWORD]}")) + return + fi +} + +function _gz_sim_flags +{ + for word in $GZ_SIM_COMPLETION_LIST; do + echo "$word" + done +} diff --git a/src/cmd/model.bash_completion.sh b/src/cmd/model.bash_completion.sh new file mode 100644 index 0000000000..6ddc4f4128 --- /dev/null +++ b/src/cmd/model.bash_completion.sh @@ -0,0 +1,55 @@ +#!/usr/bin/env bash +# +# 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. +# + +# bash tab-completion + +# This is a per-library function definition, used in conjunction with the +# top-level entry point in ign-tools. + +GZ_MODEL_COMPLETION_LIST=" + --list + -m --model + -p --pose + -l --link + -j --joint + -h --help + --force-version + --versions +" + +function _gz_model +{ + if [[ ${COMP_WORDS[COMP_CWORD]} == -* ]]; then + # Specify options (-*) word list for this subcommand + COMPREPLY=($(compgen -W "$GZ_MODEL_COMPLETION_LIST" \ + -- "${COMP_WORDS[COMP_CWORD]}" )) + return + else + # Just use bash default auto-complete, because we never have two + # subcommands in the same line. If that is ever needed, change here to + # detect subsequent subcommands + COMPREPLY=($(compgen -o default -- "${COMP_WORDS[COMP_CWORD]}")) + return + fi +} + +function _gz_model_flags +{ + for word in $GZ_MODEL_COMPLETION_LIST; do + echo "$word" + done +} diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index a1d7e71871..a66def21b3 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -19,7 +19,9 @@ #include #include +#include #include +#include #include #include @@ -29,6 +31,8 @@ static const std::string kBinPath(PROJECT_BINARY_PATH); static const std::string kIgnCommand( std::string(BREW_RUBY) + std::string(IGN_PATH) + "/ign gazebo -s "); +static const std::string kIgnModelCommand( + std::string(BREW_RUBY) + std::string(IGN_PATH) + "/ign model "); ///////////////////////////////////////////////// std::string customExecStr(std::string _cmd) @@ -160,3 +164,65 @@ TEST(CmdLine, ResourcePath) EXPECT_EQ(output.find("Unable to find file plugins.sdf"), std::string::npos) << output; } + +////////////////////////////////////////////////// +/// \brief Check --help message and bash completion script for consistent flags +TEST(CmdLine, GazeboHelpVsCompletionFlags) +{ + // Flags in help message + std::string helpOutput = customExecStr(kIgnCommand + " gazebo --help"); + + // Call the output function in the bash completion script + std::string scriptPath = ignition::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), + "src", "cmd", "gazebo.bash_completion.sh"); + + // Equivalent to: + // sh -c "bash -c \". /path/to/gazebo.bash_completion.sh; _gz_sim_flags\"" + std::string cmd = "bash -c \". " + scriptPath + "; _gz_sim_flags\""; + std::string scriptOutput = customExecStr(cmd); + + // Tokenize script output + std::istringstream iss(scriptOutput); + std::vector flags((std::istream_iterator(iss)), + std::istream_iterator()); + + EXPECT_GT(flags.size(), 0u); + + // Match each flag in script output with help message + for (const auto &flag : flags) + { + EXPECT_NE(std::string::npos, helpOutput.find(flag)) << helpOutput; + } +} + +////////////////////////////////////////////////// +/// \brief Check --help message and bash completion script for consistent flags +TEST(CmdLine, ModelHelpVsCompletionFlags) +{ + // Flags in help message + std::string helpOutput = customExecStr(kIgnModelCommand + " --help"); + + // Call the output function in the bash completion script + std::string scriptPath = ignition::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), + "src", "cmd", "model.bash_completion.sh"); + + // Equivalent to: + // sh -c "bash -c \". /path/to/model.bash_completion.sh; _gz_model_flags\"" + std::string cmd = "bash -c \". " + scriptPath + "; _gz_model_flags\""; + std::string scriptOutput = customExecStr(cmd); + + // Tokenize script output + std::istringstream iss(scriptOutput); + std::vector flags((std::istream_iterator(iss)), + std::istream_iterator()); + + EXPECT_GT(flags.size(), 0u); + + // Match each flag in script output with help message + for (const auto &flag : flags) + { + EXPECT_NE(std::string::npos, helpOutput.find(flag)) << helpOutput; + } +} From c4dca78f76d360e00a86ca65503d86b3f84138d7 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 16 Jun 2022 10:37:57 -0700 Subject: [PATCH 06/13] Fix locks in Visualize Lidar GUI plugin (#1538) Signed-off-by: Ian Chen --- src/gui/plugins/visualize_lidar/VisualizeLidar.cc | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index ab306bdd8b..1a39fcffdf 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -379,7 +379,6 @@ void VisualizeLidar::UpdateSize(int _size) ////////////////////////////////////////////////// void VisualizeLidar::OnTopic(const QString &_topicName) { - std::lock_guard(this->dataPtr->serviceMutex); if (!this->dataPtr->topicName.empty() && !this->dataPtr->node.Unsubscribe(this->dataPtr->topicName)) { @@ -388,6 +387,7 @@ void VisualizeLidar::OnTopic(const QString &_topicName) } this->dataPtr->topicName = _topicName.toStdString(); + std::lock_guard lock(this->dataPtr->serviceMutex); // Reset visualization this->dataPtr->resetVisual = true; @@ -406,14 +406,14 @@ void VisualizeLidar::OnTopic(const QString &_topicName) ////////////////////////////////////////////////// void VisualizeLidar::UpdateNonHitting(bool _value) { - std::lock_guard(this->dataPtr->serviceMutex); + std::lock_guard lock(this->dataPtr->serviceMutex); this->dataPtr->lidar->SetDisplayNonHitting(_value); } ////////////////////////////////////////////////// void VisualizeLidar::DisplayVisual(bool _value) { - std::lock_guard(this->dataPtr->serviceMutex); + std::lock_guard lock(this->dataPtr->serviceMutex); this->dataPtr->lidar->SetVisible(_value); ignmsg << "Lidar Visual Display " << ((_value) ? "ON." : "OFF.") << std::endl; @@ -422,7 +422,6 @@ void VisualizeLidar::DisplayVisual(bool _value) ///////////////////////////////////////////////// void VisualizeLidar::OnRefresh() { - std::lock_guard(this->dataPtr->serviceMutex); ignmsg << "Refreshing topic list for LaserScan messages." << std::endl; // Clear @@ -468,7 +467,7 @@ void VisualizeLidar::SetTopicList(const QStringList &_topicList) ////////////////////////////////////////////////// void VisualizeLidar::OnScan(const msgs::LaserScan &_msg) { - std::lock_guard(this->dataPtr->serviceMutex); + std::lock_guard lock(this->dataPtr->serviceMutex); if (this->dataPtr->initialized) { this->dataPtr->msg = std::move(_msg); From 3f56166dab9020b94fbce71a9e164fc7ae4b3ff9 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 17 Jun 2022 09:25:11 -0700 Subject: [PATCH 07/13] Fix clang warning from Thruster plugin (#1540) Signed-off-by: Louise Poubel Co-authored-by: Aditya Pande --- src/systems/thruster/Thruster.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 96b397ea45..6380eb7972 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -111,7 +111,7 @@ namespace systems /// Documentation inherited public: void PostUpdate(const UpdateInfo &_info, - const EntityComponentManager &_ecm); + const EntityComponentManager &_ecm) override; /// \brief Private data pointer private: std::unique_ptr dataPtr; From 481c513a3063f5795610129d5d0ea1ec6eb295e0 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 17 Jun 2022 16:20:28 -0700 Subject: [PATCH 08/13] Odometry publisher: also publish Pose_V (TF) (#1534) Signed-off-by: Louise Poubel --- .../worlds/multicopter_velocity_control.sdf | 4 ++ src/systems/diff_drive/DiffDrive.hh | 2 +- .../odometry_publisher/OdometryPublisher.cc | 64 +++++++++++++------ .../odometry_publisher/OdometryPublisher.hh | 4 ++ test/integration/odometry_publisher.cc | 47 ++++++++++++-- 5 files changed, 98 insertions(+), 23 deletions(-) diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index effbeeafff..33d45db9d0 100644 --- a/examples/worlds/multicopter_velocity_control.sdf +++ b/examples/worlds/multicopter_velocity_control.sdf @@ -16,6 +16,10 @@ You can use the velocity controller and command linear velocity and yaw angular ign topic -e -t "/model/x3/odometry" + Listen to poses: + + ign topic -e -t "/model/x3/pose" + Send commands to the hexacopter to go straight up: diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index c9d45e6617..6da5719320 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -63,7 +63,7 @@ namespace systems /// `/model/{name_of_model}/odometry`. /// /// ``: Custom topic on which this system will publish the - /// transform from `frame_id` to `child_frame_id`. This element if optional, + /// transform from `frame_id` to `child_frame_id`. This element is optional, /// and the default value is `/model/{name_of_model}/tf`. /// /// ``: Custom `frame_id` field that this system will use as the diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 8bdd6f215b..8b6270c0fd 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -74,12 +74,15 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// \brief Last sim time odom was published. public: std::chrono::steady_clock::duration lastOdomPubTime{0}; - /// \brief Diff drive odometry message publisher. + /// \brief Odometry message publisher. public: transport::Node::Publisher odomPub; - /// \brief Diff drive odometry with covariance message publisher. + /// \brief Odometry with covariance message publisher. public: transport::Node::Publisher odomCovPub; + /// \brief Pose vector (TF) message publisher. + public: transport::Node::Publisher tfPub; + /// \brief Rolling mean accumulators for the linear velocity public: std::tuple linearMean; @@ -144,7 +147,7 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->odomFrame = this->dataPtr->model.Name(_ecm) + "/" + "odom"; if (!_sdf->HasElement("odom_frame")) { - ignwarn << "OdometryPublisher system plugin missing , " + igndbg << "OdometryPublisher system plugin missing , " << "defaults to \"" << this->dataPtr->odomFrame << "\"" << std::endl; } else @@ -174,7 +177,7 @@ void OdometryPublisher::Configure(const Entity &_entity, + "/" + "base_footprint"; if (!_sdf->HasElement("robot_base_frame")) { - ignwarn << "OdometryPublisher system plugin missing , " + igndbg << "OdometryPublisher system plugin missing , " << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl; } else @@ -229,6 +232,8 @@ void OdometryPublisher::Configure(const Entity &_entity, { this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopicValid); + ignmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid + << "]" << std::endl; } std::string odomCovTopicValid { @@ -242,6 +247,26 @@ void OdometryPublisher::Configure(const Entity &_entity, { this->dataPtr->odomCovPub = this->dataPtr->node.Advertise< msgs::OdometryWithCovariance>(odomCovTopicValid); + ignmsg << "OdometryPublisher publishing odometry with covariance on [" + << odomCovTopicValid << "]" << std::endl; + } + + std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/pose"}; + if (_sdf->HasElement("tf_topic")) + tfTopic = _sdf->Get("tf_topic"); + std::string tfTopicValid {transport::TopicUtils::AsValidTopic(tfTopic)}; + if (tfTopicValid.empty()) + { + ignerr << "Failed to generate valid TF topic from [" << tfTopic << "]" + << std::endl; + } + else + { + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopicValid); + ignmsg << "OdometryPublisher publishing Pose_V (TF) on [" + << tfTopicValid << "]" << std::endl; } } @@ -399,17 +424,19 @@ void OdometryPublisherPrivate::UpdateOdometry( ignition::math::Rand::DblNormal(0, this->gaussianNoise)); // Set the time stamp in the header. - msg.mutable_header()->mutable_stamp()->CopyFrom( - convert(_info.simTime)); + msgs::Header header; + header.mutable_stamp()->CopyFrom(convert(_info.simTime)); // Set the frame ids. - auto frame = msg.mutable_header()->add_data(); + auto frame = header.add_data(); frame->set_key("frame_id"); frame->add_value(odomFrame); - auto childFrame = msg.mutable_header()->add_data(); + auto childFrame = header.add_data(); childFrame->set_key("child_frame_id"); childFrame->add_value(robotBaseFrame); + msg.mutable_header()->CopyFrom(header); + this->lastUpdatePose = pose; this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime); @@ -430,16 +457,7 @@ void OdometryPublisherPrivate::UpdateOdometry( msgs::OdometryWithCovariance msgCovariance; // Set the time stamp in the header. - msgCovariance.mutable_header()->mutable_stamp()->CopyFrom( - convert(_info.simTime)); - - // Set the frame ids. - frame = msgCovariance.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value(odomFrame); - childFrame = msg.mutable_header()->add_data(); - childFrame->set_key("child_frame_id"); - childFrame->add_value(robotBaseFrame); + msgCovariance.mutable_header()->CopyFrom(header); // Copy position from odometry msg. msgCovariance.mutable_pose_with_covariance()-> @@ -488,6 +506,16 @@ void OdometryPublisherPrivate::UpdateOdometry( { this->odomCovPub.Publish(msgCovariance); } + + if (this->tfPub.Valid()) + { + msgs::Pose_V tfMsg; + auto tfMsgPose = tfMsg.add_pose(); + tfMsgPose->CopyFrom(msg.pose()); + tfMsgPose->mutable_header()->CopyFrom(header); + + this->tfPub.Publish(tfMsg); + } } IGNITION_ADD_PLUGIN(OdometryPublisher, diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index 7fdab8253b..c087ad51a0 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -57,6 +57,10 @@ namespace systems /// odometry with covariance messages. This element is optional, and the /// default value is `/model/{name_of_model}/odometry_with_covariance`. /// + /// ``: Custom topic on which this system will publish the + /// transform from `odom_frame` to `robot_base_frame`. This element is + /// optional, and the default value is `/model/{name_of_model}/pose`. + /// /// ``: Number of dimensions to represent odometry. Only 2 and 3 /// dimensional spaces are supported. This element is optional, and the /// default value is 2. diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index d5994ac36c..52e2eb0891 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -208,8 +208,16 @@ class OdometryPublisherTest /// \param[in] _sdfFile SDF file to load. /// \param[in] _odomTopic Odometry topic. + /// \param[in] _tfTopic TF / Pose_V topic. + /// \param[in] _frameId Name of the world-fixed coordinate frame + /// for the odometry message. + /// \param[in] _childFrameId Name of the coordinate frame rigidly + /// attached to the mobile robot base. protected: void TestMovement3d(const std::string &_sdfFile, - const std::string &_odomTopic) + const std::string &_odomTopic, + const std::string &_tfTopic, + const std::string &_frameId, + const std::string &_childFrameId) { // Start server ServerConfig serverConfig; @@ -253,6 +261,7 @@ class OdometryPublisherTest std::vector odomPoses; std::vector odomLinVels; std::vector odomAngVels; + std::vector tfPoses; // Create function to store data from odometry messages std::function odomCb = [&](const msgs::Odometry &_msg) @@ -271,10 +280,30 @@ class OdometryPublisherTest odomLinVels.push_back(msgs::Convert(_msg.twist().linear())); odomAngVels.push_back(msgs::Convert(_msg.twist().angular())); }; + // Create function to store data from Pose_V messages + std::function tfCb = + [&](const msgs::Pose_V &_msg) + { + ASSERT_EQ(_msg.pose_size(), 1); + EXPECT_TRUE(_msg.pose(0).has_header()); + EXPECT_TRUE(_msg.pose(0).has_position()); + EXPECT_TRUE(_msg.pose(0).has_orientation()); + + ASSERT_EQ(_msg.pose(0).header().data_size(), 2); + + EXPECT_EQ(_msg.pose(0).header().data(0).key(), "frame_id"); + EXPECT_EQ(_msg.pose(0).header().data(0).value().Get(0), _frameId); + + EXPECT_EQ(_msg.pose(0).header().data(1).key(), "child_frame_id"); + EXPECT_EQ(_msg.pose(0).header().data(1).value().Get(0), _childFrameId); + + tfPoses.push_back(msgs::Convert(_msg.pose(0))); + }; // Create node for publishing twist messages transport::Node node; auto cmdVel = node.Advertise("/X3/gazebo/command/twist"); node.Subscribe(_odomTopic, odomCb); + node.Subscribe(_tfTopic, tfCb); test::Relay velocityRamp; math::Vector3d linVelCmd(0.5, 0.3, 1.5); @@ -299,17 +328,19 @@ class OdometryPublisherTest int sleep = 0; int maxSleep = 30; - for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + for (; (odomPoses.size() < 150 || tfPoses.size() < 150) && + sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - ASSERT_NE(maxSleep, sleep); + EXPECT_NE(maxSleep, sleep); // Odom for 3s ASSERT_FALSE(odomPoses.empty()); EXPECT_EQ(150u, odomPoses.size()); EXPECT_EQ(150u, odomLinVels.size()); EXPECT_EQ(150u, odomAngVels.size()); + EXPECT_EQ(150u, tfPoses.size()); // Check accuracy of poses published in the odometry message auto finalModelFramePose = odomPoses.back(); @@ -333,6 +364,14 @@ class OdometryPublisherTest EXPECT_NEAR(odomAngVels.back().X(), 0.0, 1e-1); EXPECT_NEAR(odomAngVels.back().Y(), 0.0, 1e-1); EXPECT_NEAR(odomAngVels.back().Z(), angVelCmd[2], 1e-1); + + // Check TF + EXPECT_NEAR(poses.back().Pos().X(), tfPoses.back().Pos().X(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Y(), tfPoses.back().Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Z(), tfPoses.back().Pos().Z(), 1e-2); + EXPECT_NEAR(poses.back().Rot().X(), tfPoses.back().Rot().X(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Y(), tfPoses.back().Rot().Y(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Z(), tfPoses.back().Rot().Z(), 1e-2); } /// \param[in] _sdfFile SDF file to load. @@ -572,7 +611,7 @@ TEST_P(OdometryPublisherTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Movement3d)) TestMovement3d( ignition::common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "odometry_publisher_3d.sdf"), - "/model/X3/odometry"); + "/model/X3/odometry", "/model/X3/pose", "X3/odom", "X3/base_footprint"); } ///////////////////////////////////////////////// From b843232791563eabe15fba69cc91a51dc5600870 Mon Sep 17 00:00:00 2001 From: Ashton Larkin <42042756+adlarkin@users.noreply.github.com> Date: Mon, 20 Jun 2022 09:16:50 -0600 Subject: [PATCH 09/13] Add bounding boxes into the label system plugin (#1040) Signed-off-by: AmrElsersy Signed-off-by: Ashton Larkin Signed-off-by: Louise Poubel Co-authored-by: AmrElsersy Co-authored-by: Louise Poubel --- CMakeLists.txt | 5 +- examples/worlds/boundingbox_camera.sdf | 415 ++++++++++++++++++ .../gazebo/components/BoundingBoxCamera.hh | 45 ++ src/SdfEntityCreator.cc | 6 + src/rendering/RenderUtil.cc | 43 ++ src/systems/sensors/CMakeLists.txt | 1 + src/systems/sensors/Sensors.cc | 15 +- 7 files changed, 527 insertions(+), 3 deletions(-) create mode 100644 examples/worlds/boundingbox_camera.sdf create mode 100644 include/ignition/gazebo/components/BoundingBoxCamera.hh diff --git a/CMakeLists.txt b/CMakeLists.txt index 4b9ff4329a..ce2bae341c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -119,7 +119,7 @@ set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-sensors -ign_find_package(ignition-sensors6 REQUIRED VERSION 6.1 +ign_find_package(ignition-sensors6 REQUIRED VERSION 6.6 # component order is important COMPONENTS # non-rendering @@ -138,6 +138,7 @@ ign_find_package(ignition-sensors6 REQUIRED VERSION 6.1 # cameras camera + boundingbox_camera segmentation_camera depth_camera rgbd_camera @@ -147,7 +148,7 @@ set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR}) #-------------------------------------- # Find ignition-rendering -ign_find_package(ignition-rendering6 REQUIRED VERSION 6.3) +ign_find_package(ignition-rendering6 REQUIRED VERSION 6.5) set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) #-------------------------------------- diff --git a/examples/worlds/boundingbox_camera.sdf b/examples/worlds/boundingbox_camera.sdf new file mode 100644 index 0000000000..a4c16af4c5 --- /dev/null +++ b/examples/worlds/boundingbox_camera.sdf @@ -0,0 +1,415 @@ + + + + + + + + + + + + ogre2 + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 1.0 1.0 1.0 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + boxes_full_2d_image + + Full 2D + docked + 400 + 600 + + + + + boxes_visible_2d_image + + Visible 2D + docked + 400 + 600 + + + + + boxes_3d_image + + 3D + docked + 400 + 600 + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 0 0 0.5 1 + 0 0 1 1 + 0 0 0.3 1 + + + + + + + + + + + + + 0 1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 0.8 0.2 0 1 + 0.8 0 0 1 + + + + + + + true + -1 -2 0.5 0 0 0 + + + + + 0.5 + + + + + + + 0.5 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + false + + + + + + + + + 4 0 1.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + boxes_full_2d + + full_2d + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + bounding_box_full_2d_data + + + 1 + 30 + true + + + + boxes_visible_2d + + visible_2d + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + bounding_box_visible_2d_data + + + 1 + 30 + true + + + + boxes_3d + + 3d + 1.047 + + 800 + 600 + + + 0.1 + 10 + + + bounding_box_3d_data + + + 1 + 30 + true + + + + + + -1 0 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + + diff --git a/include/ignition/gazebo/components/BoundingBoxCamera.hh b/include/ignition/gazebo/components/BoundingBoxCamera.hh new file mode 100644 index 0000000000..2451fd8f24 --- /dev/null +++ b/include/ignition/gazebo/components/BoundingBoxCamera.hh @@ -0,0 +1,45 @@ +/* + * 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. + * +*/ +#ifndef IGNITION_GAZEBO_COMPONENTS_BOUNDINGBOXCAMERA_HH_ +#define IGNITION_GAZEBO_COMPONENTS_BOUNDINGBOXCAMERA_HH_ + +#include + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains a BoundingBox camera sensor, + /// sdf::Camera, information. + using BoundingBoxCamera = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.BoundingBoxCamera", + BoundingBoxCamera) +} +} +} +} +#endif diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index d54e1f377f..afb49a05cb 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -27,6 +27,7 @@ #include "ignition/gazebo/components/Altimeter.hh" #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/Atmosphere.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CanonicalLink.hh" #include "ignition/gazebo/components/CastShadows.hh" @@ -868,6 +869,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::SegmentationCamera(*_sensor)); } + else if (_sensor->Type() == sdf::SensorType::BOUNDINGBOX_CAMERA) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::BoundingBoxCamera(*_sensor)); + } else if (_sensor->Type() == sdf::SensorType::AIR_PRESSURE) { this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 23348a4cf9..415908ff9c 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -52,6 +52,7 @@ #include #include "ignition/gazebo/components/Actor.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" #include "ignition/gazebo/components/ChildLinkName.hh" @@ -1644,6 +1645,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string boundingBoxCameraSuffix{"/boundingbox"}; // Get all the new worlds // TODO(anyone) Only one scene is supported for now @@ -1885,6 +1887,17 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( _parent->Data(), segmentationCameraSuffix); return true; }); + + // Create bounding box cameras + _ecm.Each( + [&](const Entity &_entity, + const components::BoundingBoxCamera *_boundingBoxCamera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _boundingBoxCamera->Data(), + _parent->Data(), boundingBoxCameraSuffix); + return true; + }); } } @@ -1898,6 +1911,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string boundingBoxCameraSuffix{"/boundingbox"}; // Get all the new worlds // TODO(anyone) Only one scene is supported for now @@ -2139,6 +2153,17 @@ void RenderUtilPrivate::CreateEntitiesRuntime( _parent->Data(), segmentationCameraSuffix); return true; }); + + // Create bounding box cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::BoundingBoxCamera *_boundingBoxCamera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _boundingBoxCamera->Data(), + _parent->Data(), boundingBoxCameraSuffix); + return true; + }); } } @@ -2299,6 +2324,16 @@ void RenderUtilPrivate::UpdateRenderingEntities( this->entityPoses[_entity] = _pose->Data(); return true; }); + + // Update bounding box cameras + _ecm.Each( + [&](const Entity &_entity, + const components::BoundingBoxCamera *, + const components::Pose *_pose)->bool + { + this->entityPoses[_entity] = _pose->Data(); + return true; + }); } ////////////////////////////////////////////////// @@ -2430,6 +2465,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // bounding box cameras + _ecm.EachRemoved( + [&](const Entity &_entity, const components::BoundingBoxCamera *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + // collisions _ecm.EachRemoved( [&](const Entity &_entity, const components::Collision *)->bool diff --git a/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index 8b56378885..7f616f6c3d 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -5,6 +5,7 @@ gz_add_system(sensors ${rendering_target} ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ignition-sensors${IGN_SENSORS_VER}::boundingbox_camera ignition-sensors${IGN_SENSORS_VER}::camera ignition-sensors${IGN_SENSORS_VER}::depth_camera ignition-sensors${IGN_SENSORS_VER}::gpu_lidar diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 7ba021d997..3a0573d58a 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -43,6 +44,7 @@ #include "ignition/gazebo/components/Atmosphere.hh" #include "ignition/gazebo/components/BatterySoC.hh" +#include "ignition/gazebo/components/BoundingBoxCamera.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" @@ -576,7 +578,8 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::GpuLidar::typeId) || _ecm.HasComponentType(components::RgbdCamera::typeId) || _ecm.HasComponentType(components::ThermalCamera::typeId) || - _ecm.HasComponentType(components::SegmentationCamera::typeId))) + _ecm.HasComponentType(components::SegmentationCamera::typeId) || + _ecm.HasComponentType(components::BoundingBoxCamera::typeId))) { igndbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; @@ -740,6 +743,11 @@ std::string Sensors::CreateSensor(const Entity &_entity, sensor = this->dataPtr->sensorManager.CreateSensor< sensors::ThermalCameraSensor>(_sdf); } + else if (_sdf.Type() == sdf::SensorType::BOUNDINGBOX_CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::BoundingBoxCameraSensor>(_sdf); + } else if (_sdf.Type() == sdf::SensorType::SEGMENTATION_CAMERA) { sensor = this->dataPtr->sensorManager.CreateSensor< @@ -861,6 +869,11 @@ bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const if (s) return s->HasConnections(); } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } { auto s = dynamic_cast(_sensor); if (s) From a74e92653954e93870a6fc7e71320fb5f74b578e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 20 Jun 2022 22:15:21 +0100 Subject: [PATCH 10/13] Update README links (#1546) Signed-off-by: ahcorde --- README.md | 6 +++--- examples/scripts/python_api/README.md | 2 +- tutorials/log.md | 2 +- tutorials/migration_link_api.md | 10 +++++----- tutorials/migration_model_api.md | 10 +++++----- tutorials/migration_world_api.md | 8 ++++---- tutorials/physics.md | 2 +- tutorials/resources.md | 10 +++++----- 8 files changed, 25 insertions(+), 25 deletions(-) diff --git a/README.md b/README.md index e949910270..56b06ab46d 100644 --- a/README.md +++ b/README.md @@ -78,7 +78,7 @@ introspection and control. # Install -See the [installation tutorial](https://ignitionrobotics.org/api/gazebo/6.1/install.html). +See the [installation tutorial](https://gazebosim.org/api/gazebo/6.9/install.html). # Usage @@ -125,11 +125,11 @@ This issue is tracked [here](https://github.com/ignitionrobotics/ign-tools/issue # Documentation -See the [installation tutorial](https://ignitionrobotics.org/api/gazebo/6.0/install.html). +See the [installation tutorial](https://gazebosim.org/api/gazebo/6.9/install.html). # Testing -See the [installation tutorial](https://ignitionrobotics.org/api/gazebo/6.0/install.html). +See the [installation tutorial](https://gazebosim.org/api/gazebo/6.9/install.html). See the [Writing Tests section of the contributor guide](https://github.com/ignitionrobotics/ign-gazebo/blob/main/CONTRIBUTING.md#writing-tests) for help creating or modifying tests. diff --git a/examples/scripts/python_api/README.md b/examples/scripts/python_api/README.md index 620e2bb7be..d0bc19590d 100644 --- a/examples/scripts/python_api/README.md +++ b/examples/scripts/python_api/README.md @@ -3,4 +3,4 @@ This example shows how to use Gazebo's Python API. For more information, see the -[Python interfaces](https://ignitionrobotics.org/api/gazebo/6.9/python_interfaces.html) tutorial. +[Python interfaces](https://gazebosim.org/api/gazebo/6.9/python_interfaces.html) tutorial. diff --git a/tutorials/log.md b/tutorials/log.md index 09423fa8a7..2de76ff76a 100644 --- a/tutorials/log.md +++ b/tutorials/log.md @@ -42,7 +42,7 @@ Other options for recording: ### From C++ API All features available through the command line are also available through -[ignition::gazebo::ServerConfig](https://ignitionrobotics.org/api/gazebo/2.0/classignition_1_1gazebo_1_1ServerConfig.html). +[ignition::gazebo::ServerConfig](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1ServerConfig.html). When instantiating a server programmatically, logging options can be passed to the constructor, for example: diff --git a/tutorials/migration_link_api.md b/tutorials/migration_link_api.md index 5d290a0f03..8a610a9c87 100644 --- a/tutorials/migration_link_api.md +++ b/tutorials/migration_link_api.md @@ -34,14 +34,14 @@ can be divided in these categories: You'll find the Ignition APIs below on the following headers: -* [ignition/gazebo/Link.hh](https://ignitionrobotics.org/api/gazebo/3.3/Link_8hh.html) -* [ignition/gazebo/Util.hh](https://ignitionrobotics.org/api/gazebo/3.3/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://ignitionrobotics.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [ignition/gazebo/Link.hh](https://gazebosim.org/api/gazebo/6.9/Link_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6.9/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6.9/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. The functions presented here exist for convenience and readability. ### Properties diff --git a/tutorials/migration_model_api.md b/tutorials/migration_model_api.md index e3a4a5337e..de79a30bc0 100644 --- a/tutorials/migration_model_api.md +++ b/tutorials/migration_model_api.md @@ -34,14 +34,14 @@ can be divided in these categories: You'll find the Ignition APIs below on the following headers: -* [ignition/gazebo/Model.hh](https://ignitionrobotics.org/api/gazebo/3.3/Model_8hh.html) -* [ignition/gazebo/Util.hh](https://ignitionrobotics.org/api/gazebo/3.3/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://ignitionrobotics.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) -* [ignition/gazebo/EntityComponentManager.hh](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [ignition/gazebo/Model.hh](https://gazebosim.org/api/gazebo/6.9/Model_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6.9/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6.9/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1EntityComponentManager.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. The functions presented here exist for convenience and readability. ### Properties diff --git a/tutorials/migration_world_api.md b/tutorials/migration_world_api.md index f4fdc3b974..a377214b35 100644 --- a/tutorials/migration_world_api.md +++ b/tutorials/migration_world_api.md @@ -34,13 +34,13 @@ can be divided in these categories: You'll find the Ignition APIs below on the following headers: -* [ignition/gazebo/World.hh](https://ignitionrobotics.org/api/gazebo/3.3/World_8hh.html) -* [ignition/gazebo/Util.hh](https://ignitionrobotics.org/api/gazebo/3.3/Util_8hh.html) -* [ignition/gazebo/SdfEntityCreator.hh](https://ignitionrobotics.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) +* [ignition/gazebo/World.hh](https://gazebosim.org/api/gazebo/6.9/World_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6.9/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6.9/SdfEntityCreator_8hh.html) It's worth remembering that most of this functionality can be performed using the -[EntityComponentManager](https://ignitionrobotics.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. The functions presented here exist for convenience and readability. ### Properties diff --git a/tutorials/physics.md b/tutorials/physics.md index 4ff2a32097..e1cac1a129 100644 --- a/tutorials/physics.md +++ b/tutorials/physics.md @@ -73,7 +73,7 @@ Alternatively, you can choose a plugin from the command line using the ### From C++ API All features available through the command line are also available through -[ignition::gazebo::ServerConfig](https://ignitionrobotics.org/api/gazebo/2.0/classignition_1_1gazebo_1_1ServerConfig.html). +[ignition::gazebo::ServerConfig](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1ServerConfig.html). When instantiating a server programmatically, a physics engine can be passed to the constructor, for example: diff --git a/tutorials/resources.md b/tutorials/resources.md index 430f1f4216..a086a4fba4 100644 --- a/tutorials/resources.md +++ b/tutorials/resources.md @@ -31,16 +31,16 @@ System plugins may be loaded through: * Attached to a **model**: `` * Attached to a **sensor**: `` * Passing the shared library and class to be loaded through - [PluginInfo](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1ServerConfig_1_1PluginInfo.html) - (within [ServerConfig](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1ServerConfig.html)) + [PluginInfo](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1ServerConfig_1_1PluginInfo.html) + (within [ServerConfig](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1ServerConfig.html)) when instantiating the - [Server](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1Server.html#a084ef7616f5af42061a7aeded5651ab0). + [Server](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1Server.html#a084ef7616f5af42061a7aeded5651ab0). Ignition will look for system plugins on the following paths, in order: 1. All paths on the `IGN_GAZEBO_SYSTEM_PLUGIN_PATH` environment variable 2. `$HOME/.ignition/gazebo/plugins` -3. [Systems that are installed with Ignition Gazebo](https://ignitionrobotics.org/api/gazebo/4.6/namespaceignition_1_1gazebo_1_1systems.html) +3. [Systems that are installed with Ignition Gazebo](https://gazebosim.org/api/gazebo/6.9/namespaceignition_1_1gazebo_1_1systems.html) ### Ignition GUI plugins @@ -111,7 +111,7 @@ Top-level entities such as models, lights and actors may be loaded through: * Path / URL to SDF file * (TODO) `ignition::msgs::Model`, `ignition::msgs::Light` * Within a system, using - [SdfEntityCreator](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1SdfEntityCreator.html) + [SdfEntityCreator](https://gazebosim.org/api/gazebo/6.9/classignition_1_1gazebo_1_1SdfEntityCreator.html) or directly creating components and entities. Ignition will look for URIs (path / URL) in the following, in order: From 4a3af79e77413881d894a5ef900a013cc14e9ed7 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 21 Jun 2022 09:14:25 -0700 Subject: [PATCH 11/13] Depend on common 4.5.1 (#1547) Signed-off-by: Louise Poubel --- CMakeLists.txt | 2 +- src/cmd/cmdgazebo.rb.in | 4 ++-- src/cmd/gazebo.bash_completion.sh | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ba778b9c5e..a566322711 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -80,7 +80,7 @@ set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- # Find ignition-common # Always use the profiler component to get the headers, regardless of status. -ign_find_package(ignition-common4 VERSION 4.4 +ign_find_package(ignition-common4 VERSION 4.5.1 COMPONENTS profiler events diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 7e2e8195ec..a8937648b0 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -73,8 +73,6 @@ COMMANDS = { 'gazebo' => " enable console logging to a console.log \n"\ " file in the specified path. \n"\ "\n"\ - " --headless-rendering Run rendering in headless mode \n"\ - "\n"\ " --record-resources Implicitly invokes --record, and records \n"\ " meshes and material files, in addition to \n"\ " states and console messages. \n"\ @@ -109,6 +107,8 @@ COMMANDS = { 'gazebo' => " --playback [arg] Use logging system to play back states. \n"\ " Argument is path to recorded states. \n"\ "\n"\ + " --headless-rendering Run rendering in headless mode \n"\ + "\n"\ " -r Run simulation on start. \n"\ "\n"\ " -s Run only the server (headless mode). This \n"\ diff --git a/src/cmd/gazebo.bash_completion.sh b/src/cmd/gazebo.bash_completion.sh index bea403004f..72ad2ac6ff 100644 --- a/src/cmd/gazebo.bash_completion.sh +++ b/src/cmd/gazebo.bash_completion.sh @@ -24,7 +24,6 @@ GZ_SIM_COMPLETION_LIST=" -g --iterations --levels - --headless-rendering --network-role --network-secondaries --record @@ -34,6 +33,7 @@ GZ_SIM_COMPLETION_LIST=" --log-overwrite --log-compress --playback + --headless-rendering -r -s -v --verbose From 9c60c162e119040df3a86e01ad833750c92ec4b5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 21 Jun 2022 11:08:39 -0700 Subject: [PATCH 12/13] Disable CLI tests on Windows Signed-off-by: Louise Poubel --- src/ign_TEST.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index 8412a7d75c..fbf01162bf 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -168,7 +168,7 @@ TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) ////////////////////////////////////////////////// /// \brief Check --help message and bash completion script for consistent flags -TEST(CmdLine, GazeboHelpVsCompletionFlags) +TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(GazeboHelpVsCompletionFlags)) { // Flags in help message std::string helpOutput = customExecStr(kIgnCommand + " gazebo --help"); @@ -199,7 +199,7 @@ TEST(CmdLine, GazeboHelpVsCompletionFlags) ////////////////////////////////////////////////// /// \brief Check --help message and bash completion script for consistent flags -TEST(CmdLine, ModelHelpVsCompletionFlags) +TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelHelpVsCompletionFlags)) { // Flags in help message std::string helpOutput = customExecStr(kIgnModelCommand + " --help"); From 0516e5adfd77ed390df2906654504afad4ac3b01 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Thu, 4 Aug 2022 17:26:42 -0700 Subject: [PATCH 13/13] ign to gz (#1630) Signed-off-by: Louise Poubel --- examples/worlds/boundingbox_camera.sdf | 64 +++++++++---------- .../worlds/multicopter_velocity_control.sdf | 2 +- src/gz_TEST.cc | 8 +-- .../odometry_publisher/OdometryPublisher.cc | 11 ++-- tutorials/resources.md | 4 +- 5 files changed, 43 insertions(+), 46 deletions(-) diff --git a/examples/worlds/boundingbox_camera.sdf b/examples/worlds/boundingbox_camera.sdf index a4c16af4c5..eb7c7afd86 100644 --- a/examples/worlds/boundingbox_camera.sdf +++ b/examples/worlds/boundingbox_camera.sdf @@ -5,20 +5,20 @@ + filename="gz-sim-physics-system" + name="gz::sim::systems::Physics"> + filename="gz-sim-user-commands-system" + name="gz::sim::systems::UserCommands"> + filename="gz-sim-scene-broadcaster-system" + name="gz::sim::systems::SceneBroadcaster"> + filename="gz-sim-sensors-system" + name="gz::sim::systems::Sensors"> ogre2 @@ -26,11 +26,11 @@ - + 3D View false docked - + ogre2 scene @@ -41,43 +41,43 @@ - + floating 5 5 false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + false 5 5 floating false - + - + World control false false @@ -90,7 +90,7 @@ - + true true @@ -101,7 +101,7 @@ - + World stats false false @@ -114,7 +114,7 @@ - + true true @@ -124,32 +124,32 @@ boxes_full_2d_image - + Full 2D docked 400 600 - + boxes_visible_2d_image - + Visible 2D docked 400 600 - + boxes_3d_image - + 3D docked 400 600 - + @@ -228,7 +228,7 @@ 0 0 1 1 0 0 0.3 1 - + @@ -236,7 +236,7 @@ - + 0 1 0.5 0 0 0 @@ -300,7 +300,7 @@ false - + @@ -405,7 +405,7 @@ https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone - + diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index 689793272b..5092471eca 100644 --- a/examples/worlds/multicopter_velocity_control.sdf +++ b/examples/worlds/multicopter_velocity_control.sdf @@ -18,7 +18,7 @@ You can use the velocity controller and command linear velocity and yaw angular Listen to poses: - ign topic -e -t "/model/x3/pose" + gz topic -e -t "/model/x3/pose" Send commands to the hexacopter to go straight up: diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index e343c2d2d7..633b837ad8 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -171,15 +171,15 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(GazeboHelpVsCompletionFlags)) { // Flags in help message - std::string helpOutput = customExecStr(kGzCommand + " gazebo --help"); + std::string helpOutput = customExecStr(kGzCommand + " sim --help"); // Call the output function in the bash completion script std::string scriptPath = gz::common::joinPaths( std::string(PROJECT_SOURCE_PATH), - "src", "cmd", "gazebo.bash_completion.sh"); + "src", "cmd", "sim.bash_completion.sh"); // Equivalent to: - // sh -c "bash -c \". /path/to/gazebo.bash_completion.sh; _gz_sim_flags\"" + // sh -c "bash -c \". /path/to/sim.bash_completion.sh; _gz_sim_flags\"" std::string cmd = "bash -c \". " + scriptPath + "; _gz_sim_flags\""; std::string scriptOutput = customExecStr(cmd); @@ -193,7 +193,7 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(GazeboHelpVsCompletionFlags)) // Match each flag in script output with help message for (const auto &flag : flags) { - EXPECT_NE(std::string::npos, helpOutput.find(flag)) << helpOutput; + EXPECT_NE(std::string::npos, helpOutput.find(flag)) << flag; } } diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index c6f29d9b96..b9d4e09369 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -233,7 +233,7 @@ void OdometryPublisher::Configure(const Entity &_entity, { this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopicValid); - ignmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid + gzmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid << "]" << std::endl; } @@ -248,25 +248,24 @@ void OdometryPublisher::Configure(const Entity &_entity, { this->dataPtr->odomCovPub = this->dataPtr->node.Advertise< msgs::OdometryWithCovariance>(odomCovTopicValid); - ignmsg << "OdometryPublisher publishing odometry with covariance on [" + gzmsg << "OdometryPublisher publishing odometry with covariance on [" << odomCovTopicValid << "]" << std::endl; } - std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/pose"}; + std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/pose"}; if (_sdf->HasElement("tf_topic")) tfTopic = _sdf->Get("tf_topic"); std::string tfTopicValid {transport::TopicUtils::AsValidTopic(tfTopic)}; if (tfTopicValid.empty()) { - ignerr << "Failed to generate valid TF topic from [" << tfTopic << "]" + gzerr << "Failed to generate valid TF topic from [" << tfTopic << "]" << std::endl; } else { this->dataPtr->tfPub = this->dataPtr->node.Advertise( tfTopicValid); - ignmsg << "OdometryPublisher publishing Pose_V (TF) on [" + gzmsg << "OdometryPublisher publishing Pose_V (TF) on [" << tfTopicValid << "]" << std::endl; } } diff --git a/tutorials/resources.md b/tutorials/resources.md index 67b772d3ec..09634ed9b7 100644 --- a/tutorials/resources.md +++ b/tutorials/resources.md @@ -111,9 +111,7 @@ Top-level entities such as models, lights and actors may be loaded through: * Path / URL to SDF file * (TODO) `gz::msgs::Model`, `gz::msgs::Light` * Within a system, using -==== BASE ==== - [SdfEntityCreator](https://ignitionrobotics.org/api/gazebo/4.6/classignition_1_1gazebo_1_1SdfEntityCreator.html) -==== BASE ==== + [SdfEntityCreator](https://gazebosim.org/api/gazebo/4.6/classignition_1_1gazebo_1_1SdfEntityCreator.html) or directly creating components and entities. Gazebo will look for URIs (path / URL) in the following, in order: