diff --git a/CMakeLists.txt b/CMakeLists.txt index 77e6209e20..8eff281618 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -140,6 +140,7 @@ gz_find_package(gz-sensors7 REQUIRED # cameras camera + boundingbox_camera segmentation_camera depth_camera rgbd_camera @@ -166,6 +167,9 @@ if (GZ_TOOLS_PROGRAM) else() message (STATUS "Searching for gz program - not found. CLI tests are skipped.") endif() +# Note that CLI files are installed regardless of whether the dependency is +# available during build time +set(GZ_TOOLS_VER 2) #-------------------------------------- # Find gz-utils diff --git a/Changelog.md b/Changelog.md index 95d87f03b5..dabf31fada 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1907,6 +1907,63 @@ ### Gazebo Sim 3.X.X (20XX-XX-XX) +### Gazebo Sim 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) + ### Gazebo Sim 3.12.0 (2021-11-11) 1. Prevent creation of spurious `` elements when saving worlds diff --git a/examples/worlds/boundingbox_camera.sdf b/examples/worlds/boundingbox_camera.sdf new file mode 100644 index 0000000000..eb7c7afd86 --- /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/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index 2677c0c79f..5092471eca 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 gz topic -e -t "/model/x3/odometry" + Listen to poses: + + gz topic -e -t "/model/x3/pose" + Send commands to the hexacopter to go straight up: diff --git a/include/gz/sim/components/BoundingBoxCamera.hh b/include/gz/sim/components/BoundingBoxCamera.hh new file mode 100644 index 0000000000..496d6ff513 --- /dev/null +++ b/include/gz/sim/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 GZ_SIM_COMPONENTS_BOUNDINGBOXCAMERA_HH_ +#define GZ_SIM_COMPONENTS_BOUNDINGBOXCAMERA_HH_ + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains a BoundingBox camera sensor, + /// sdf::Camera, information. + using BoundingBoxCamera = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.BoundingBoxCamera", + BoundingBoxCamera) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/BoundingBoxCamera.hh b/include/ignition/gazebo/components/BoundingBoxCamera.hh new file mode 100644 index 0000000000..2bb86098d6 --- /dev/null +++ b/include/ignition/gazebo/components/BoundingBoxCamera.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 7661b77afb..b768861033 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -27,6 +27,7 @@ #include "gz/sim/components/Altimeter.hh" #include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/Atmosphere.hh" +#include "gz/sim/components/BoundingBoxCamera.hh" #include "gz/sim/components/Camera.hh" #include "gz/sim/components/CanonicalLink.hh" #include "gz/sim/components/CastShadows.hh" @@ -869,6 +870,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::WIDE_ANGLE_CAMERA) { this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index a5fd5070a2..2e073c0a52 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -1,7 +1,7 @@ #=============================================================================== # Generate the ruby script. # Note that the major version of the library is included in the name. -# Ex: cmdgazebo0.rb +# Ex: cmdsim0.rb set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") set(cmd_script_configured "${cmd_script_generated}.configured") @@ -25,7 +25,7 @@ set(gz_library_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/gz/cmd${GZ_DESIGNATION}${P # Generate a configuration file. # Note that the major version of the library is included in the name. -# Ex: gazebo0.yaml +# Ex: sim0.yaml configure_file( "${GZ_DESIGNATION}.yaml.in" "${CMAKE_CURRENT_BINARY_DIR}/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml" @ONLY) @@ -67,7 +67,7 @@ install(FILES ${model_configured} DESTINATION ${CMAKE_INSTALL_PREFIX}/${CMAKE_IN #=============================================================================== # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. -# Ex: cmdgazebo0.rb +# Ex: cmdsim0.rb set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/ruby/gz/cmd${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") set(cmd_script_configured_test "${cmd_script_generated_test}.configured") @@ -90,7 +90,7 @@ set(gz_library_path # Generate a configuration file for internal testing. # Note that the major version of the library is included in the name. -# Ex: gazebo0.yaml +# Ex: sim0.yaml configure_file( "${GZ_DESIGNATION}.yaml.in" "${CMAKE_BINARY_DIR}/test/conf/${GZ_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml" @ONLY) @@ -117,3 +117,25 @@ set(gz_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( + "sim.bash_completion.sh" + "${CMAKE_CURRENT_BINARY_DIR}/sim${PROJECT_VERSION_MAJOR}.bash_completion.sh" @ONLY) +install( + FILES + ${CMAKE_CURRENT_BINARY_DIR}/sim${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/cmdsim.rb.in b/src/cmd/cmdsim.rb.in index 5d0ff59123..37ccc70cd0 100755 --- a/src/cmd/cmdsim.rb.in +++ b/src/cmd/cmdsim.rb.in @@ -73,8 +73,6 @@ COMMANDS = { 'sim' => " 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 = { 'sim' => " --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"\ @@ -161,6 +161,8 @@ COMMANDS = { 'sim' => " GZ_SIM_SERVER_CONFIG_PATH Path to server configuration file. \n\n"\ " GZ_GUI_PLUGIN_PATH Colon separated paths used to locate GUI \n"\ " plugins. \n"\ + " GZ_GUI_RESOURCE_PATH Colon separated paths used to locate GUI \n"\ + " resources such as configuration files. \n\n" } # 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/cmd/sim.bash_completion.sh b/src/cmd/sim.bash_completion.sh new file mode 100644 index 0000000000..d2d53705a3 --- /dev/null +++ b/src/cmd/sim.bash_completion.sh @@ -0,0 +1,73 @@ +#!/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 gz-tools. + +GZ_SIM_COMPLETION_LIST=" + -g + --iterations + --levels + --network-role + --network-secondaries + --record + --record-path + --record-resources + --record-topic + --log-overwrite + --log-compress + --playback + --headless-rendering + -r + -s + -v --verbose + --gui-config + --physics-engine + --render-engine + --render-engine-gui + --render-engine-server + --version + -z + -h --help + --force-version + --versions +" + +function _gz_sim +{ + if [[ ${COMP_WORDS[COMP_CWORD]} == -* ]]; then + # Specify options (-*) word list for this subcommand + 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/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 6f4cbfbb90..b8a1007fe7 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); gzmsg << "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); gzmsg << "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); diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index f7c004d7c0..633b837ad8 100644 --- a/src/gz_TEST.cc +++ b/src/gz_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 kGzCommand( std::string(BREW_RUBY) + std::string(GZ_PATH) + " sim -s "); +static const std::string kGzModelCommand( + std::string(BREW_RUBY) + std::string(GZ_PATH) + " model "); ///////////////////////////////////////////////// std::string customExecStr(std::string _cmd) @@ -161,3 +165,65 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(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, GZ_UTILS_TEST_DISABLED_ON_WIN32(GazeboHelpVsCompletionFlags)) +{ + // Flags in help message + 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", "sim.bash_completion.sh"); + + // Equivalent to: + // 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); + + // 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)) << flag; + } +} + +////////////////////////////////////////////////// +/// \brief Check --help message and bash completion script for consistent flags +TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(ModelHelpVsCompletionFlags)) +{ + // Flags in help message + std::string helpOutput = customExecStr(kGzModelCommand + " --help"); + + // Call the output function in the bash completion script + std::string scriptPath = gz::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; + } +} diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 3ed94509d8..093155bf21 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -52,6 +52,7 @@ #include #include "gz/sim/components/Actor.hh" +#include "gz/sim/components/BoundingBoxCamera.hh" #include "gz/sim/components/Camera.hh" #include "gz/sim/components/CastShadows.hh" #include "gz/sim/components/ChildLinkName.hh" @@ -1658,6 +1659,7 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string boundingBoxCameraSuffix{"/boundingbox"}; const std::string wideAngleCameraSuffix{"/image"}; // Get all the new worlds @@ -1900,6 +1902,17 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( 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; + }); + // Create wide angle cameras _ecm.Each( [&](const Entity &_entity, @@ -1923,6 +1936,7 @@ void RenderUtilPrivate::CreateEntitiesRuntime( const std::string thermalCameraSuffix{"/image"}; const std::string gpuLidarSuffix{"/scan"}; const std::string segmentationCameraSuffix{"/segmentation"}; + const std::string boundingBoxCameraSuffix{"/boundingbox"}; const std::string wideAngleCameraSuffix{"/image"}; // Get all the new worlds @@ -2166,6 +2180,17 @@ void RenderUtilPrivate::CreateEntitiesRuntime( 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; + }); + // Create wide angle cameras _ecm.EachNew( [&](const Entity &_entity, @@ -2337,6 +2362,16 @@ void RenderUtilPrivate::UpdateRenderingEntities( 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; + }); + // Update wide angle cameras _ecm.Each( [&](const Entity &_entity, @@ -2477,6 +2512,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; + }); + // wide angle cameras _ecm.EachRemoved( [&](const Entity &_entity, const components::WideAngleCamera *)->bool diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index 0208695b42..150621bb7c 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 150791271f..b9d4e09369 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -75,12 +75,15 @@ class gz::sim::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; @@ -145,7 +148,7 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->odomFrame = this->dataPtr->model.Name(_ecm) + "/" + "odom"; if (!_sdf->HasElement("odom_frame")) { - gzwarn << "OdometryPublisher system plugin missing , " + gzdbg << "OdometryPublisher system plugin missing , " << "defaults to \"" << this->dataPtr->odomFrame << "\"" << std::endl; } else @@ -175,7 +178,7 @@ void OdometryPublisher::Configure(const Entity &_entity, + "/" + "base_footprint"; if (!_sdf->HasElement("robot_base_frame")) { - gzwarn << "OdometryPublisher system plugin missing , " + gzdbg << "OdometryPublisher system plugin missing , " << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl; } else @@ -230,6 +233,8 @@ void OdometryPublisher::Configure(const Entity &_entity, { this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopicValid); + gzmsg << "OdometryPublisher publishing odometry on [" << odomTopicValid + << "]" << std::endl; } std::string odomCovTopicValid { @@ -243,6 +248,25 @@ void OdometryPublisher::Configure(const Entity &_entity, { this->dataPtr->odomCovPub = this->dataPtr->node.Advertise< msgs::OdometryWithCovariance>(odomCovTopicValid); + gzmsg << "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()) + { + gzerr << "Failed to generate valid TF topic from [" << tfTopic << "]" + << std::endl; + } + else + { + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopicValid); + gzmsg << "OdometryPublisher publishing Pose_V (TF) on [" + << tfTopicValid << "]" << std::endl; } } @@ -400,17 +424,19 @@ void OdometryPublisherPrivate::UpdateOdometry( gz::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); @@ -431,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()-> @@ -489,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); + } } GZ_ADD_PLUGIN(OdometryPublisher, diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index 16d84f0891..f7b462da7c 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/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index f7a299c1b4..7d68172129 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -5,6 +5,7 @@ gz_add_system(sensors ${rendering_target} gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} + gz-sensors${GZ_SENSORS_VER}::boundingbox_camera gz-sensors${GZ_SENSORS_VER}::camera gz-sensors${GZ_SENSORS_VER}::depth_camera gz-sensors${GZ_SENSORS_VER}::gpu_lidar diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a5582fa9b7..b1191b2f16 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -32,6 +32,7 @@ #include #include +#include #include #include #include @@ -44,6 +45,7 @@ #include "gz/sim/components/Atmosphere.hh" #include "gz/sim/components/BatterySoC.hh" +#include "gz/sim/components/BoundingBoxCamera.hh" #include "gz/sim/components/Camera.hh" #include "gz/sim/components/DepthCamera.hh" #include "gz/sim/components/GpuLidar.hh" @@ -613,6 +615,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && (this->dataPtr->forceUpdate || + _ecm.HasComponentType(components::BoundingBoxCamera::typeId) || _ecm.HasComponentType(components::Camera::typeId) || _ecm.HasComponentType(components::DepthCamera::typeId) || _ecm.HasComponentType(components::GpuLidar::typeId) || @@ -792,6 +795,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< diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh index 4abf4ff4a9..2202ab9397 100644 --- a/test/integration/ModelPhotoShootTest.hh +++ b/test/integration/ModelPhotoShootTest.hh @@ -94,14 +94,21 @@ void testImages(const std::string &_imageFile, EXPECT_EQ(image.Width(), testImage.Width()); EXPECT_EQ(image.Height(), testImage.Height()); EXPECT_EQ(image.PixelFormat(), testImage.PixelFormat()); - unsigned int dataLength; - unsigned char *imageData = nullptr; - image.Data(&imageData, dataLength); - unsigned int testDataLenght; - unsigned char *testImageData = nullptr; - image.Data(&testImageData, testDataLenght); - ASSERT_EQ(dataLength, testDataLenght); - ASSERT_EQ(memcmp(imageData, testImageData, dataLength), 0); + // Images should be almost equal (They might have + // minimal color differences on a few pixels) + unsigned int equalPixels = 0; + unsigned int totalPixels = testImage.Width() * testImage.Height(); + for (unsigned int x = 0; x < image.Width(); x++) + { + for (unsigned int y = 0; y < image.Height(); y++) + { + if (image.Pixel(x, y) == testImage.Pixel(x, y)) + { + equalPixels++; + } + } + } + ASSERT_GT((float)equalPixels/(float)totalPixels, 0.99); // Deleting files so they do not affect future tests EXPECT_EQ(remove(imageFilePath.c_str()), 0); diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index 1c148a5ea4..5ad89520fb 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, GZ_UTILS_TEST_DISABLED_ON_WIN32(Movement3d)) TestMovement3d( gz::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"); } ///////////////////////////////////////////////// diff --git a/test/integration/sensors_system_battery.cc b/test/integration/sensors_system_battery.cc index 84f7fa7109..d160331d97 100644 --- a/test/integration/sensors_system_battery.cc +++ b/test/integration/sensors_system_battery.cc @@ -192,8 +192,8 @@ TEST_F(SensorsFixture, GZ_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; }