From cac78d9e0993118878bd9c9eb671df7ecb7c03d6 Mon Sep 17 00:00:00 2001 From: Silvio Date: Tue, 5 Jul 2022 21:10:16 +0200 Subject: [PATCH 01/10] Enable use of ign gazebo -s on Windows Signed-off-by: Silvio --- src/CMakeLists.txt | 32 ++++++++-- src/cmd/CMakeLists.txt | 121 +++++++++++++++++++++---------------- src/cmd/ModelCommandAPI.hh | 6 +- src/cmd/cmdgazebo.rb.in | 16 ++++- src/cmd/cmdmodel.rb.in | 4 +- src/ign.hh | 16 ++--- src/ign_TEST.cc | 13 ++-- src/systems/CMakeLists.txt | 7 +-- 8 files changed, 136 insertions(+), 79 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index ff178007c3..7acb368963 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -92,7 +92,6 @@ set (gtest_sources TestFixture_TEST.cc Util_TEST.cc World_TEST.cc - ign_TEST.cc comms/Broker_TEST.cc comms/MsgManager_TEST.cc network/NetworkConfig_TEST.cc @@ -100,11 +99,22 @@ set (gtest_sources network/NetworkManager_TEST.cc ) +# ign_TEST and ModelCommandAPI_TEST are not supported with multi config +# CMake generators, see also cmd/CMakeLists.txt +get_property(GENERATOR_IS_MULTI_CONFIG GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) +if(NOT GENERATOR_IS_MULTI_CONFIG) + list(APPEND gtest_sources ign_TEST.cc) +endif() + + # Tests that require a valid display set(tests_needing_display - ModelCommandAPI_TEST.cc ) +if(NOT GENERATOR_IS_MULTI_CONFIG) + list(APPEND tests_needing_display ModelCommandAPI_TEST.cc) +endif() + # Add systems that need a valid display here. # \todo(anyone) Find a way to run these tests with a virtual display such Xvfb # or Xdummy instead of skipping them @@ -222,9 +232,19 @@ foreach(CMD_TEST set_tests_properties(${CMD_TEST} PROPERTIES ENVIRONMENT "${_env_vars}") -endforeach() + # On Windows there is no RPATH, so an alternative way for tests for finding .dll libraries + # in build directory in necessary. For regular tests, the trick is to place all libraries + # and executables in a common CMAKE_RUNTIME_OUTPUT_DIRECTORY, so that the .dll are found + # as they are in the same directory where the executable is loaded. For tests that are + # launched via ruby, this does not work, so we need to manually add CMAKE_RUNTIME_OUTPUT_DIRECTORY + # to the PATH. This is done via the ENVIRONMENT_MODIFICATION that is only available + # since CMake 3.22 . However, if an older CMake is used another trick to install the libraries + # beforehand + if (WIN32 AND CMAKE_VERSION STRGREATER "3.22") + set_tests_properties(${CMD_TEST} PROPERTIES + ENVIRONMENT_MODIFICATION "PATH=path_list_prepend:${CMAKE_RUNTIME_OUTPUT_DIRECTORY}") + endif() -if(NOT WIN32) - add_subdirectory(cmd) -endif() +endforeach() +add_subdirectory(cmd) diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index 11e7ef7e00..e81a98be44 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -2,12 +2,19 @@ # Generate the ruby script. # Note that the major version of the library is included in the name. # Ex: cmdgazebo0.rb -set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmd${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") -set(cmd_script_configured "${cmd_script_generated}.configured") +set(cmd_script_name "cmd${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") +set(cmd_script_generated "${CMAKE_CURRENT_BINARY_DIR}/$_${cmd_script_name}") +set(cmd_script_configured "${CMAKE_CURRENT_BINARY_DIR}/${cmd_script_name}.configured") # Set the library_location variable to the relative path to the library file # within the install directory structure. -set(library_location "../../../${CMAKE_INSTALL_LIBDIR}/$") +if(WIN32) + set(plugin_location ${CMAKE_INSTALL_BINDIR}) +else() + set(plugin_location ${CMAKE_INSTALL_LIBDIR}) +endif() + +set(library_location "../../../${plugin_location}/$") configure_file( "cmd${IGN_DESIGNATION}.rb.in" @@ -19,7 +26,7 @@ file(GENERATE INPUT "${cmd_script_configured}") # Install the ruby command line library in an unversioned location. -install(FILES ${cmd_script_generated} DESTINATION lib/ruby/ignition) +install(FILES ${cmd_script_generated} DESTINATION lib/ruby/ignition RENAME ${cmd_script_name}) set(ign_library_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/ignition/cmd${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}") @@ -40,8 +47,9 @@ install( FILES # Used for the installed model command version. # Generate the ruby script that gets installed. # Note that the major version of the library is included in the name. -set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") -set(cmd_model_script_configured "${cmd_model_script_generated}.configured") +set(cmd_model_script_name "cmdmodel${PROJECT_VERSION_MAJOR}.rb") +set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/$_${cmd_model_script_name}") +set(cmd_model_script_configured "${CMAKE_CURRENT_BINARY_DIR}/${cmd_script_name}.configured") configure_file( "cmdmodel.rb.in" @@ -51,7 +59,7 @@ file(GENERATE OUTPUT "${cmd_model_script_generated}" INPUT "${cmd_model_script_configured}") -install(FILES ${cmd_model_script_generated} DESTINATION lib/ruby/ignition) +install(FILES ${cmd_model_script_generated} DESTINATION lib/ruby/ignition RENAME ${cmd_model_script_name}) # Used for the installed version. set(ign_model_ruby_path "${CMAKE_INSTALL_PREFIX}/lib/ruby/ignition/cmdmodel${PROJECT_VERSION_MAJOR}") @@ -68,55 +76,64 @@ 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 -set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/ruby/ignition/cmd${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") -set(cmd_script_configured_test "${cmd_script_generated_test}.configured") - -# Set the library_location variable to the relative path to the library file -# within the install directory structure. -set(library_location "$") - -configure_file( - "cmd${IGN_DESIGNATION}.rb.in" - "${cmd_script_configured_test}" - @ONLY) - -file(GENERATE - OUTPUT "${cmd_script_generated_test}" - INPUT "${cmd_script_configured_test}") - -# Used only for internal testing. -set(ign_library_path - "${CMAKE_BINARY_DIR}/test/lib/ruby/ignition/cmd${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}") - -# Generate a configuration file for internal testing. -# Note that the major version of the library is included in the name. -# Ex: gazebo0.yaml -configure_file( - "${IGN_DESIGNATION}.yaml.in" - "${CMAKE_BINARY_DIR}/test/conf/${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml" @ONLY) +# The logic is valid only for single-config CMake generators, so no script is +# generated if a multiple-config CMake geneator is used +get_property(GENERATOR_IS_MULTI_CONFIG GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) +if(NOT GENERATOR_IS_MULTI_CONFIG) + set(cmd_script_generated_test "${CMAKE_BINARY_DIR}/test/lib/ruby/ignition/cmd${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}.rb") + set(cmd_script_configured_test "${cmd_script_generated_test}.configured") + + # Set the library_location variable to the relative path to the library file + # within the install directory structure. + set(library_location "$") + + configure_file( + "cmd${IGN_DESIGNATION}.rb.in" + "${cmd_script_configured_test}" + @ONLY) + + file(GENERATE + OUTPUT "${cmd_script_generated_test}" + INPUT "${cmd_script_configured_test}") + + # Used only for internal testing. + set(ign_library_path + "${CMAKE_BINARY_DIR}/test/lib/ruby/ignition/cmd${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}") + + # Generate a configuration file for internal testing. + # Note that the major version of the library is included in the name. + # Ex: gazebo0.yaml + configure_file( + "${IGN_DESIGNATION}.yaml.in" + "${CMAKE_BINARY_DIR}/test/conf/${IGN_DESIGNATION}${PROJECT_VERSION_MAJOR}.yaml" @ONLY) +endif() #=============================================================================== # Generate the ruby script for internal testing. # Note that the major version of the library is included in the name. -set(cmd_model_ruby_test_dir "${CMAKE_BINARY_DIR}/test/lib/ruby/ignition") -set(cmd_model_script_generated_test "${cmd_model_ruby_test_dir}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") -set(cmd_model_script_configured_test "${cmd_model_script_generated_test}.configured") - -configure_file( - "cmdmodel.rb.in" - "${cmd_model_script_configured_test}" - @ONLY) - -file(GENERATE - OUTPUT "${cmd_model_script_generated_test}" - INPUT "${cmd_model_script_configured_test}") - -# Used for internal testing. -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) +# The logic is valid only for single-config CMake generators, so no script is +# generated if a multiple-config CMake geneator is used +if(NOT GENERATOR_IS_MULTI_CONFIG) + set(cmd_model_ruby_test_dir "${CMAKE_BINARY_DIR}/test/lib/ruby/ignition") + set(cmd_model_script_generated_test "${cmd_model_ruby_test_dir}/cmdmodel${PROJECT_VERSION_MAJOR}.rb") + set(cmd_model_script_configured_test "${cmd_model_script_generated_test}.configured") + + configure_file( + "cmdmodel.rb.in" + "${cmd_model_script_configured_test}" + @ONLY) + + file(GENERATE + OUTPUT "${cmd_model_script_generated_test}" + INPUT "${cmd_model_script_configured_test}") + + # Used for internal testing. + 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) +endif() #=============================================================================== # Bash completion diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh index 2ca7248c60..c941936471 100644 --- a/src/cmd/ModelCommandAPI.hh +++ b/src/cmd/ModelCommandAPI.hh @@ -15,8 +15,10 @@ * */ +#include "ignition/gazebo/Export.hh" + /// \brief External hook to get a list of available models. -extern "C" void cmdModelList(); +extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelList(); /// \brief External hook to dump model information. /// \param[in] _modelName Model name. @@ -24,7 +26,7 @@ extern "C" void cmdModelList(); /// \param[in] _linkName Link name. /// \param[in] _jointName Joint name. /// \param[in] _sensorName Sensor name. -extern "C" void cmdModelInfo( +extern "C" IGNITION_GAZEBO_VISIBLE void cmdModelInfo( const char *_modelName, int _pose, const char *_linkName, const char *_jointName, const char *_sensorName); diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 16ee5e1cac..023843ce1d 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -27,6 +27,7 @@ end require 'optparse' require 'erb' +require 'pathname' # Constants. LIBRARY_NAME = '@library_location@' @@ -337,7 +338,8 @@ class Cmd def execute(args) options = parse(args) - if LIBRARY_NAME[0] == '/' + library_name_path = Pathname.new(LIBRARY_NAME) + if library_name_path.absolute? # If the first character is a slash, we'll assume that we've been given an # absolute path to the library. This is only used during test mode. plugin = LIBRARY_NAME @@ -467,6 +469,12 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." exit(-1) end + if plugin.end_with? ".dll" + puts "`ign gazebo` currently only works with the -s argument on Windows. +See https://github.com/gazebosim/gz-sim/issues/168 for more info." + exit(-1) + end + serverPid = Process.fork do ENV['RMT_PORT'] = '1500' Process.setpgid(0, 0) @@ -526,6 +534,12 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." exit(-1) end + if plugin.end_with? ".dll" + puts "`ign gazebo` currently only works with the -s argument on Windows. +See https://github.com/gazebosim/gz-sim/issues/168 for more info." + exit(-1) + end + ENV['RMT_PORT'] = '1501' Importer.runGui(options['gui_config'], options['file'], options['wait_gui'], options['render_engine_gui']) diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index 88e65a50f3..e49eb30749 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -26,6 +26,7 @@ else end require 'optparse' +require 'pathname' # Constants. LIBRARY_NAME = '@library_location@' @@ -157,7 +158,8 @@ class Cmd options = parse(args) # Read the plugin that handles the command. - if LIBRARY_NAME[0] == '/' + library_name_path = Pathname.new(LIBRARY_NAME) + if library_name_path.absolute? # If the first character is a slash, we'll assume that we've been given an # absolute path to the library. This is only used during test mode. plugin = LIBRARY_NAME diff --git a/src/ign.hh b/src/ign.hh index 38de2a88e6..18eac3cfc5 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -21,18 +21,18 @@ /// \brief External hook to read the library version. /// \return C-string representing the version. Ex.: 0.1.2 -extern "C" char *ignitionGazeboVersion(); +extern "C" IGNITION_GAZEBO_VISIBLE char *ignitionGazeboVersion(); /// \brief Get the Gazebo version header. /// \return C-string containing the Gazebo version information. -extern "C" char *gazeboVersionHeader(); +extern "C" IGNITION_GAZEBO_VISIBLE char *gazeboVersionHeader(); /// \brief Set verbosity level /// \param[in] _verbosity 0 to 4 -extern "C" void cmdVerbosity( +extern "C" IGNITION_GAZEBO_VISIBLE void cmdVerbosity( const char *_verbosity); -extern "C" const char *worldInstallDir(); +extern "C" IGNITION_GAZEBO_VISIBLE const char *worldInstallDir(); /// \brief External hook to run simulation server. /// \param[in] _sdfString SDF file to run, as a string. @@ -59,7 +59,7 @@ extern "C" const char *worldInstallDir(); /// \param[in] _headless True if server rendering should run headless /// \param[in] _recordPeriod --record-period option /// \return 0 if successful, 1 if not. -extern "C" int runServer(const char *_sdfString, +extern "C" IGNITION_GAZEBO_VISIBLE int runServer(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, int _recordResources, int _logOverwrite, @@ -77,14 +77,14 @@ extern "C" int runServer(const char *_sdfString, /// it receives a world path from GUI. /// \param[in] _renderEngine --render-engine-gui option /// \return 0 if successful, 1 if not. -extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, - const char *_renderEngine); +extern "C" IGNITION_GAZEBO_VISIBLE int runGui(const char *_guiConfig, + const char *_file, int _waitGui, const char *_renderEngine); /// \brief External hook to find or download a fuel world provided a URL. /// \param[in] _pathToResource Path to the fuel world resource, ie, /// https://staging-fuel.ignitionrobotics.org/1.0/gmas/worlds/ShapesClone /// \return C-string containing the path to the local world sdf file -extern "C" const char *findFuelResource( +extern "C" IGNITION_GAZEBO_VISIBLE const char *findFuelResource( char *_pathToResource); #endif diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index fbf01162bf..397441bed4 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -59,8 +59,7 @@ std::string customExecStr(std::string _cmd) } ///////////////////////////////////////////////// -// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 -TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(Server)) +TEST(CmdLine, Server) { std::string cmd = kIgnCommand + " -r -v 4 --iterations 5 " + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf"; @@ -75,6 +74,9 @@ TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(Server)) << output; } +// Disable on WIN32 as on Windows it is not support to prepend +// a command with the env variable to set +#ifndef _WIN32 // Use IGN_GAZEBO_RESOURCE_PATH instead of specifying the complete path cmd = std::string("IGN_GAZEBO_RESOURCE_PATH=") + PROJECT_SOURCE_PATH + "/test/worlds " + kIgnCommand + @@ -89,10 +91,11 @@ TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(Server)) EXPECT_NE(output.find("iteration " + std::to_string(i)), std::string::npos) << output; } +#endif } ///////////////////////////////////////////////// -TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(CachedFuelWorld)) +TEST(CmdLine, CachedFuelWorld) { std::string projectPath = std::string(PROJECT_SOURCE_PATH) + "/test/worlds"; ignition::common::setenv("IGN_FUEL_CACHE_PATH", projectPath.c_str()); @@ -106,7 +109,7 @@ TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(CachedFuelWorld)) } ///////////////////////////////////////////////// -TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(GazeboServer)) +TEST(CmdLine, GazeboServer) { std::string cmd = kIgnCommand + " -r -v 4 --iterations 5 " + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf"; @@ -123,7 +126,7 @@ TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(GazeboServer)) } ///////////////////////////////////////////////// -TEST(CmdLine, IGN_UTILS_TEST_DISABLED_ON_WIN32(Gazebo)) +TEST(CmdLine, Gazebo) { std::string cmd = kIgnCommand + " -r -v 4 --iterations 5 " + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf"; diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 303c6fc7d9..330b267f25 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -82,10 +82,9 @@ function(gz_add_system system_name) set(unversioned ${CMAKE_SHARED_LIBRARY_PREFIX}${PROJECT_NAME_NO_VERSION_LOWER}-${system_name}${CMAKE_SHARED_LIBRARY_SUFFIX}) if(WIN32) # symlinks on Windows require admin priviledges, fallback to copy - ADD_CUSTOM_COMMAND(TARGET ${system_target} POST_BUILD - COMMAND "${CMAKE_COMMAND}" -E copy - "$" - "$/${unversioned}") + INSTALL(CODE "EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E copy + ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}\/${versioned} + ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}\/${unversioned})") else() file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/lib") EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned} WORKING_DIRECTORY "${PROJECT_BINARY_DIR}/lib") From 0a477031b87858a257bde985e70265fe9a747dec Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Mon, 29 Aug 2022 08:53:19 +0200 Subject: [PATCH 02/10] Update src/CMakeLists.txt MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Silvio --- src/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7acb368963..d4ea8bc402 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -236,9 +236,9 @@ foreach(CMD_TEST # in build directory in necessary. For regular tests, the trick is to place all libraries # and executables in a common CMAKE_RUNTIME_OUTPUT_DIRECTORY, so that the .dll are found # as they are in the same directory where the executable is loaded. For tests that are - # launched via ruby, this does not work, so we need to manually add CMAKE_RUNTIME_OUTPUT_DIRECTORY + # launched via Ruby, this does not work, so we need to manually add CMAKE_RUNTIME_OUTPUT_DIRECTORY # to the PATH. This is done via the ENVIRONMENT_MODIFICATION that is only available - # since CMake 3.22 . However, if an older CMake is used another trick to install the libraries + # since CMake 3.22. However, if an older CMake is used another trick to install the libraries # beforehand if (WIN32 AND CMAKE_VERSION STRGREATER "3.22") set_tests_properties(${CMD_TEST} PROPERTIES From df428659cc6603df15babb17b30197bc4e8604af Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Mon, 29 Aug 2022 07:41:55 +0100 Subject: [PATCH 03/10] Add information about system paramter (#1671) Adding some API docs for changes from https://github.com/gazebosim/gz-sim/pull/1076 Signed-off-by: Kenji Brameld Signed-off-by: Silvio --- src/systems/joint_state_publisher/JointStatePublisher.hh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index 75c13f15a7..4ea973ddd9 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -35,7 +35,7 @@ namespace systems { /// \brief The JointStatePub system publishes state information for /// a model. The published message type is ignition::msgs::Model, and the - /// publication topic is "/world//model//state". + /// publication topic is determined by the `` parameter. /// /// By default the JointStatePublisher will publish all joints for /// a model. Use the `` system parameter, described below, to @@ -43,6 +43,9 @@ namespace systems /// /// # System Parameters /// + /// ``: Name of the topic to publish to. This parameter is optional, + /// and if not provided, the joint state will be published to + /// "/world//model//state". /// ``: Name of a joint to publish. This parameter can be /// specified multiple times, and is optional. All joints in a model will /// be published if joint names are not specified. From e7f8b8072e1822d860846be2f9106803fe819c19 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Wed, 31 Aug 2022 00:24:17 +0200 Subject: [PATCH 04/10] Add topic parameter to thrust plugin (#1681) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add topic parameter. Signed-off-by: Carlos Agüero Signed-off-by: Silvio --- src/systems/thruster/Thruster.cc | 21 +++++++++++++++++++-- src/systems/thruster/Thruster.hh | 4 +++- test/integration/thruster.cc | 26 +++++++++++++++++--------- test/worlds/thruster_battery.sdf | 1 + 4 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 92a74bb3a7..b775cf26e4 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -104,6 +104,9 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Diameter of propeller in m, default: 0.02 public: double propellerDiameter = 0.02; + /// \brief Topic name used to control thrust. + public: std::string topic = ""; + /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); @@ -171,6 +174,13 @@ void Thruster::Configure( this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } + // Get a custom topic. + if (_sdf->HasElement("topic")) + { + this->dataPtr->topic = transport::TopicUtils::AsValidTopic( + _sdf->Get("topic")); + } + this->dataPtr->jointEntity = model.JointByName(_ecm, jointName); if (kNullEntity == this->dataPtr->jointEntity) { @@ -191,14 +201,21 @@ void Thruster::Configure( std::string thrusterTopicOld = ignition::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/cmd_pos"); + ignwarn << thrusterTopicOld << " topic is deprecated" << std::endl; + this->dataPtr->node.Subscribe( thrusterTopicOld, &ThrusterPrivateData::OnCmdThrust, this->dataPtr.get()); // Subscribe to force commands - std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( - "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"); + std::string thrusterTopic = + "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"; + + if (!this->dataPtr->topic.empty()) + thrusterTopic = ns + "/" + this->dataPtr->topic; + + thrusterTopic = transport::TopicUtils::AsValidTopic(thrusterTopic); this->dataPtr->node.Subscribe( thrusterTopic, diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 6380eb7972..415fa8b660 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -40,8 +40,10 @@ namespace systems /// /// ## System Parameters /// - - The namespace in which the robot exists. The plugin will - /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`. + /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust` + /// or on {namespace}/{topic} if {topic} is set. /// [Optional] + /// - - The topic for receiving thrust commands. [Optional] /// - - This is the joint in the model which corresponds to the /// propeller. [Required] /// - - The fluid density of the liquid in which the thruster diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 1d28df8d5d..1e147bfe69 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -43,19 +43,20 @@ class ThrusterTest : public InternalFixture<::testing::Test> /// \brief Test a world file /// \param[in] _world Path to world file /// \param[in] _namespace Namespace for topic + /// \param[in] _topic Thrust topic /// \param[in] _coefficient Thrust coefficient /// \param[in] _density Fluid density /// \param[in] _diameter Propeller diameter /// \param[in] _baseTol Base tolerance for most quantities public: void TestWorld(const std::string &_world, - const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol); + const std::string &_namespace, const std::string &_topic, + double _coefficient, double _density, double _diameter, double _baseTol); }; ////////////////////////////////////////////////// void ThrusterTest::TestWorld(const std::string &_world, - const std::string &_namespace, double _coefficient, double _density, - double _diameter, double _baseTol) + const std::string &_namespace, const std::string &_topic, + double _coefficient, double _density, double _diameter, double _baseTol) { // Start server ServerConfig serverConfig; @@ -122,8 +123,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // Publish command and check that vehicle moved transport::Node node; - auto pub = node.Advertise( - "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust"); + auto pub = node.Advertise(_topic); int sleep{0}; int maxSleep{30}; @@ -234,31 +234,39 @@ void ThrusterTest::TestWorld(const std::string &_world, // See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PIDControl)) { + const std::string ns{"sub"}; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_pid.sdf"); // Tolerance could be lower (1e-6) if the joint pose had a precise 180 // rotation - this->TestWorld(world, "sub", 0.004, 1000, 0.2, 1e-4); + this->TestWorld(world, ns, topic, 0.004, 1000, 0.2, 1e-4); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) { + const std::string ns = "custom"; + const std::string topic = "/model/" + ns + + "/joint/propeller_joint/cmd_thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_vel_cmd.sdf"); // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2); + this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2); } ///////////////////////////////////////////////// TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) { + const std::string ns = "lowbattery"; + const std::string topic = ns + "/thrust"; auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "thruster_battery.sdf"); // Tolerance is high because the joint command disturbs the vehicle body - this->TestWorld(world, "lowbattery", 0.005, 950, 0.25, 1e-2); + this->TestWorld(world, ns, topic, 0.005, 950, 0.25, 1e-2); } diff --git a/test/worlds/thruster_battery.sdf b/test/worlds/thruster_battery.sdf index 3cf5a3495e..0a76710429 100644 --- a/test/worlds/thruster_battery.sdf +++ b/test/worlds/thruster_battery.sdf @@ -105,6 +105,7 @@ true 300 0 + thrust Date: Tue, 30 Aug 2022 19:16:32 -0700 Subject: [PATCH 05/10] =?UTF-8?q?=F0=9F=8E=88=206.12.0:=20bumped=20minor?= =?UTF-8?q?=20and=20updated=20changelog=20(#1682)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * bumped minor and updated changelog Signed-off-by: Dharini Dutia * fixed changelog as per feedback and updated migration guide Signed-off-by: Dharini Dutia Signed-off-by: Dharini Dutia Signed-off-by: Silvio --- CMakeLists.txt | 2 +- Changelog.md | 14 ++++++++++++++ Migration.md | 6 ++++++ 3 files changed, 21 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5efa74bd84..897ea591a0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.11.0) +project(ignition-gazebo6 VERSION 6.12.0) set (GZ_DISTRIBUTION "Fortress") #============================================================================ diff --git a/Changelog.md b/Changelog.md index d9bb90509e..2487f3b1ce 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,19 @@ ## Ignition Gazebo 6.x +### Gazebo Sim 6.12.0 (2022-08-30) + +1. Add topic parameter to thrust plugin + * [Pull request #1681](https://github.com/gazebosim/gz-sim/pull/1681) + +1. Add information about `` system parameter + * [Pull request #1671](https://github.com/gazebosim/gz-sim/pull/1671) + +1. Adding tests for hydrodynamics + * [Pull request #1617](https://github.com/gazebosim/gz-sim/pull/1617) + +1. Fix Windows and Doxygen + * [Pull request #1643](https://github.com/gazebosim/gz-sim/pull/1643) + ### Gazebo Sim 6.11.0 (2022-08-17) 1. Add support for specifying log record period diff --git a/Migration.md b/Migration.md index 58a4e5c815..ea3bdeea7c 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,12 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Gazebo 6.11.X to 6.12.X + + * **Modified**: + + In the Hydrodynamics plugin, inverted the added mass contribution to make it + act in the correct direction. + ## Ignition Gazebo 6.1 to 6.2 * If no `` is given to the `Thruster` plugin, the namespace now From 0aa66b26367cbe74c06aa68e6676b774b250f3f2 Mon Sep 17 00:00:00 2001 From: Kenji Brameld Date: Thu, 1 Sep 2022 16:29:57 +0900 Subject: [PATCH 06/10] Fix reference link in ackermann steering (#1683) Signed-off-by: Kenji Brameld Signed-off-by: Silvio --- src/systems/ackermann_steering/AckermannSteering.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index 11aacd9159..19b3f8d18c 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -111,7 +111,7 @@ namespace systems /// right_steering_joint /// /// References: - /// https://github.com/ignitionrobotics/ign-gazebo/tree/main/src/systems/diff_drive + /// https://github.com/gazebosim/gz-sim/tree/main/src/systems/ackermann_steering /// https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf /// https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ From 70aa84c9238f60c64367c787181b322bf0e55fdc Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Fri, 2 Sep 2022 20:38:37 +0200 Subject: [PATCH 07/10] Fix installation instructions on Ubuntu 22.04 (#1686) Signed-off-by: Silvio Traversaro Signed-off-by: Silvio --- tutorials/install.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tutorials/install.md b/tutorials/install.md index 3260f10154..1b1694f402 100644 --- a/tutorials/install.md +++ b/tutorials/install.md @@ -61,7 +61,7 @@ feature which hasn't been released yet. 1. Install tools ``` - sudo apt install -y build-essential cmake g++-8 git gnupg lsb-release wget + sudo apt install -y build-essential cmake git gnupg lsb-release wget ``` 2. Enable the Ignition software repositories: From bd1c5b6589b9986ce9be055d40bbb5358766ddb1 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Wed, 7 Sep 2022 09:23:55 -0700 Subject: [PATCH 08/10] Add a service to trigger functionality (#1611) * initial commit to allow plugin to call a service Signed-off-by: Liam Han * adding tutorial and modifying the world sdf Signed-off-by: Liam Han * added test for single input and single service output Signed-off-by: Liam Han * added test for single input and multiple service output Signed-off-by: Liam Han * added test for invalid matching service name => timeout Signed-off-by: Liam Han * modified variables the camelCase Signed-off-by: Liam Han * fixed typo, indentation, grammar, lines that exceeded 80 char Signed-off-by: Liam Han * fixing ubuntu bionic ci issue Signed-off-by: Liam Han * silly syntax mistake on expect_eq Signed-off-by: Liam Han * added three more test cases that addesses incorrect response type, incorrect request type and false result Signed-off-by: Liam Han * WIP: major restructuring and currently working. Requires more cleanup and test Signed-off-by: Liam Han * WIP: fixed preprocessor define bug Signed-off-by: Liam Han * WIP: working but extremely convoluted Signed-off-by: Liam Han * WIP major modification but a lot of errors and tests failed Signed-off-by: Liam Han * stable version: had to revert back to previous work. all tests passed Signed-off-by: Liam Han * modified to use blocking Request method as well as reduce a service worker thread to just one thread with the publisher. all tests passed Signed-off-by: Liam Han * stable version: had to revert back to previous work. all tests passed Signed-off-by: Liam Han * successfully reverted and tested Signed-off-by: Liam Han * fixing PR suggestions Signed-off-by: Liam Han * changed string with 'serv' to 'srv' and included to the header Signed-off-by: Liam Han * fixed indentation and removed rep.set_data since it's unused on the client service Signed-off-by: Liam Han * getting rid of the id Signed-off-by: Liam Han * fixed race condition resulting seldom test failure Signed-off-by: Liam Han * changed from triggerSrv to serviceCount. This compensates for the two threads running at different rate Signed-off-by: Liam Han * braces indentation Signed-off-by: Mabel Zhang * addressing gnu c compiler (gcc) warnings Signed-off-by: Liam Han Signed-off-by: Liam Han Signed-off-by: Mabel Zhang Co-authored-by: Mabel Zhang Signed-off-by: Silvio --- examples/worlds/triggered_publisher.sdf | 35 ++- .../triggered_publisher/TriggeredPublisher.cc | 158 ++++++++-- .../triggered_publisher/TriggeredPublisher.hh | 57 +++- test/integration/triggered_publisher.cc | 269 ++++++++++++++++++ test/worlds/triggered_publisher.sdf | 161 +++++++++-- tutorials/triggered_publisher.md | 39 ++- 6 files changed, 664 insertions(+), 55 deletions(-) diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 303f78beca..98b7c52c64 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -387,13 +387,17 @@ start falling. true - + body box1 box_body /box1/detach - + body box2 box_body @@ -448,19 +452,40 @@ start falling. - + linear: {x: 3} - + + + + linear: {x: 0} + + + + + data: true - + -7.5 diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 94a63b7ac8..cc7a4f88ad 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -252,7 +252,9 @@ FullMatcher::FullMatcher(const std::string &_msgType, bool _logicType, : InputMatcher(_msgType), logicType(_logicType) { if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + { return; + } this->valid = google::protobuf::TextFormat::ParseFromString( _matchString, this->matchMsg.get()); @@ -273,7 +275,9 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, fieldName(_fieldName) { if (nullptr == this->matchMsg || !this->matchMsg->IsInitialized()) + { return; + } transport::ProtoMsg *matcherSubMsg{nullptr}; if (!FindFieldSubMessage(this->matchMsg.get(), _fieldName, @@ -294,7 +298,9 @@ FieldMatcher::FieldMatcher(const std::string &_msgType, bool _logicType, } if (nullptr == matcherSubMsg) + { return; + } bool result = google::protobuf::TextFormat::ParseFieldValueFromString( _fieldString, this->fieldDescMatcher.back(), matcherSubMsg); @@ -548,7 +554,9 @@ void TriggeredPublisher::Configure(const Entity &, { int ms = sdfClone->Get("delay_ms"); if (ms > 0) + { this->delay = std::chrono::milliseconds(ms); + } } if (sdfClone->HasElement("output")) @@ -595,9 +603,52 @@ void TriggeredPublisher::Configure(const Entity &, } } } - else + + if (sdfClone->HasElement("service")) { - ignerr << "No ouptut specified" << std::endl; + for (auto serviceElem = sdfClone->GetElement("service"); serviceElem; + serviceElem = serviceElem->GetNextElement("service")) + { + SrvOutputInfo serviceInfo; + serviceInfo.srvName = serviceElem->Get("name"); + if (serviceInfo.srvName.empty()) + { + ignerr << "Service name cannot be empty\n"; + return; + } + serviceInfo.reqType = serviceElem->Get("reqType"); + if (serviceInfo.reqType.empty()) + { + ignerr << "Service request type cannot be empty\n"; + return; + } + serviceInfo.repType = serviceElem->Get("repType"); + if (serviceInfo.repType.empty()) + { + ignerr << "Service reply type cannot be empty\n"; + return; + } + serviceInfo.reqMsg = serviceElem->Get("reqMsg"); + if (serviceInfo.reqMsg.empty()) + { + ignerr << "Service request message cannot be empty\n"; + return; + } + std::string timeoutInfo = serviceElem->Get("timeout"); + if (timeoutInfo.empty()) + { + ignerr << "Timeout value cannot be empty\n"; + return; + } + + serviceInfo.timeout = std::stoi(timeoutInfo); + this->srvOutputInfo.push_back(std::move(serviceInfo)); + } + } + if (!sdfClone->HasElement("service") && !sdfClone->HasElement("output")) + { + ignerr << "No output and service specified. Make sure to specify at least" + "one of them." << std::endl; return; } @@ -606,23 +657,27 @@ void TriggeredPublisher::Configure(const Entity &, { if (this->MatchInput(_msg)) { + if (this->delay > 0ms) + { + std::lock_guard lock(this->publishQueueMutex); + this->publishQueue.push_back(this->delay); + } + else { - if (this->delay > 0ms) - { - std::lock_guard lock(this->publishQueueMutex); - this->publishQueue.push_back(this->delay); - } - else { - { - std::lock_guard lock(this->publishCountMutex); - ++this->publishCount; - } - this->newMatchSignal.notify_one(); + std::lock_guard lock(this->publishCountMutex); + ++this->publishCount; } + this->newMatchSignal.notify_one(); + } + if (this->srvOutputInfo.size() > 0) + { + std::lock_guard lock(this->triggerSrvMutex); + ++this->serviceCount; } } }); + if (!this->node.Subscribe(this->inputTopic, msgCb)) { ignerr << "Input subscriber could not be created for topic [" @@ -645,11 +700,69 @@ void TriggeredPublisher::Configure(const Entity &, std::thread(std::bind(&TriggeredPublisher::DoWork, this)); } +////////////////////////////////////////////////// +void TriggeredPublisher::PublishMsg(std::size_t pending) +{ + for (auto &info : this->outputInfo) + { + for (std::size_t i = 0; i < pending; ++i) + { + info.pub.Publish(*info.msgData); + } + } +} + +////////////////////////////////////////////////// +void TriggeredPublisher::CallService(std::size_t pendingSrv) +{ + for (auto &serviceInfo : this->srvOutputInfo) + { + for (std::size_t i = 0; i < pendingSrv; ++i) + { + bool result; + auto req = msgs::Factory::New(serviceInfo.reqType, serviceInfo.reqMsg); + if (!req) + { + ignerr << "Unable to create request for type [" + << serviceInfo.reqType << "].\n"; + return; + } + + auto rep = msgs::Factory::New(serviceInfo.repType); + if (!rep) + { + ignerr << "Unable to create response for type [" + << serviceInfo.repType << "].\n"; + return; + } + + bool executed = this->node.Request(serviceInfo.srvName, *req, + serviceInfo.timeout, *rep, result); + if (executed) + { + if (!result) + { + ignerr << "Service call [" << serviceInfo.srvName << "] failed\n"; + } + else + { + ignmsg << "Service call [" << serviceInfo.srvName << "] succeeded\n"; + } + } + else + { + ignerr << "Service call [" << serviceInfo.srvName << "] timed out\n"; + } + } + } +} + ////////////////////////////////////////////////// void TriggeredPublisher::DoWork() { while (!this->done) { + // check whether to publish a msg by checking publishCount std::size_t pending{0}; { using namespace std::chrono_literals; @@ -661,18 +774,25 @@ void TriggeredPublisher::DoWork() }); if (this->publishCount == 0 || this->done) + { continue; - + } std::swap(pending, this->publishCount); } - for (auto &info : this->outputInfo) + PublishMsg(pending); + + // check whether to call a service by checking serviceCount + std::size_t pendingSrv{0}; { - for (std::size_t i = 0; i < pending; ++i) - { - info.pub.Publish(*info.msgData); + std::lock_guard lock(this->triggerSrvMutex); + if (this->serviceCount == 0 || this->done){ + continue; } + std::swap(pendingSrv, this->serviceCount); } + + CallService(pendingSrv); } } @@ -712,7 +832,9 @@ void TriggeredPublisher::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } if (notify) + { this->newMatchSignal.notify_one(); + } } ////////////////////////////////////////////////// diff --git a/src/systems/triggered_publisher/TriggeredPublisher.hh b/src/systems/triggered_publisher/TriggeredPublisher.hh index f524b119ab..2560e67722 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.hh +++ b/src/systems/triggered_publisher/TriggeredPublisher.hh @@ -20,7 +20,7 @@ #include #include #include - +#include #include #include "ignition/gazebo/System.hh" @@ -37,8 +37,10 @@ namespace systems /// \brief The triggered publisher system publishes a user specified message /// on an output topic in response to an input message that matches user - /// specified criteria. An optional simulation time delay can be used - /// delay message publication. + /// specified criteria. It can also call a user specified service as an + /// output in response to an input message. It currently supports blocking + /// service call. An optional simulation time delay can be used delay message + /// publication. /// /// ## System Parameters /// @@ -75,9 +77,19 @@ namespace systems /// the human-readable representation of a protobuf message as used by /// `ign topic` for publishing messages /// - /// ``: Integer number of milliseconds, in simulation time, to + /// - ``: Integer number of milliseconds, in simulation time, to /// delay publication. /// + /// - ``: Contains configuration for service to call: Multiple + /// `` tags are possible. A service will be called for each input + /// that matches. + /// * Attributes: + /// * `name`: Service name (eg. `/world/triggered_publisher/set_pose`) + /// * `timeout`: Service timeout + /// * `reqType`: Service request message type (eg. ignition.msgs.Pose) + /// * `repType`: Service response message type (eg. ignition.msgs.Empty) + /// * `reqMsg`: String used to construct the service protobuf message. + /// /// Examples: /// 1. Any receipt of a Boolean messages on the input topic triggers an output /// \code{.xml} @@ -182,6 +194,12 @@ namespace systems /// \brief Thread that handles publishing output messages public: void DoWork(); + /// \brief Method that calls a service + private: void CallService(std::size_t pendingSrv); + + /// \brief Method that publishes a message + private: void PublishMsg(std::size_t pending); + /// \brief Helper function that calls Match on every InputMatcher available /// \param[in] _inputMsg Input message /// \return True if all of the matchers return true @@ -210,21 +228,49 @@ namespace systems transport::Node::Publisher pub; }; + /// \brief Class that holds necessary bits for each specified service output + private: struct SrvOutputInfo + { + /// \brief Service name + std::string srvName; + + /// \brief Service request type + std::string reqType; + + /// \brief Service reply type + std::string repType; + + /// \brief Service request message + std::string reqMsg; + + /// \brief Serivce timeout + int timeout; + }; + /// \brief List of InputMatchers private: std::vector> matchers; /// \brief List of outputs private: std::vector outputInfo; + /// \brief List of service outputs + private: std::vector srvOutputInfo; + /// \brief Ignition communication node. private: transport::Node node; + /// \brief Counter that tells how manny times to call the service + private: std::size_t serviceCount{0}; + /// \brief Counter that tells the publisher how many times to publish private: std::size_t publishCount{0}; /// \brief Mutex to synchronize access to publishCount private: std::mutex publishCountMutex; + /// \brief Mutex to synchronize access to serviceCount + private: std::mutex triggerSrvMutex; + /// \brief Condition variable to signal that new matches have occured private: std::condition_variable newMatchSignal; @@ -244,9 +290,8 @@ namespace systems /// \brief Mutex to synchronize access to publishQueue private: std::mutex publishQueueMutex; }; - } } } } - +} #endif diff --git a/test/integration/triggered_publisher.cc b/test/integration/triggered_publisher.cc index 9590a7c421..3ae1070d12 100644 --- a/test/integration/triggered_publisher.cc +++ b/test/integration/triggered_publisher.cc @@ -637,3 +637,272 @@ TEST_F(TriggeredPublisherTest, EXPECT_EQ(0u, recvCount); } + +///////////////////////////////////////////////// +/// Test for invalid service name. A service, `/srv-dummy-test` is advertised +/// and the callback is also provided in this test. Everytime an input msg is +/// published to `/in_14` topic, triggered_publisher plugin will call the +/// service `srv-test`, specified in the test/worlds/triggered_publisher.sdf. +/// However, since the two service names do not match, the callback provided in +/// this test will not be invoked. Therefore, the pubCount, which is set to 10, +/// will not equal to recvCount. The recvCount will be 0, since the callback +/// isn't invoked. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InvalidServiceName)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-dummy-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(recvCount, 0u); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call in response to an input msg. A service, +/// `srv-test` is advertised and the callback is also provided in this test. +/// Everytime an input msg is published to `/in_14` topic, triggered_publisher +/// plugin will call the service `/srv-test`, specified in the test/worlds/ +/// triggered_publisher.sdf. This time, the name of the services match. By +/// publishing input msg 10 times, the service callback will also be invoked 10 +/// times. The `pubCount` is set to 10 and recvCount is increased everytime +/// request data matches the string "test" inside the service callback. For a +/// successful test, the pubCount will equal to recvCount. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(InputMessagesTriggerServices)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_14"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering multiple services (in sequence) in response to an input +/// msg by publishing 10 times. Two services, `srv-test-0` and `srv-test-1` are +/// specified in the test/worlds/triggered_publisher.sdf. Everytime an input msg +/// is published, triggered_publisher will call the service and invoke the +/// callback. std::vector is passed as a reference and will be populated with +/// the request message, which will be a boolean value of `true`. If successful, +/// `recvMsg0` and `recvMsg1` vectors should both have a size of 10 with all +/// true boolean values. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MultipleServiceForOneInput)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_15"); + std::mutex recvMsgMutex; + std::vector recvMsgs0; + std::vector recvMsgs1; + auto cbCreator = [&recvMsgMutex](std::vector &_msgVector) + { + return std::function( + [&_msgVector, &recvMsgMutex](const auto &req, auto &) + { + std::lock_guard lock(recvMsgMutex); + if (req.data()) + { + _msgVector.push_back(req.data()); + return true; + } + return false; + }); + }; + + auto msgCb0 = cbCreator(recvMsgs0); + auto msgCb1 = cbCreator(recvMsgs1); + + // Advertise two dummy services + node.Advertise("/srv-test-0", msgCb0); + node.Advertise("/srv-test-1", msgCb1); + + const int pubCount{10}; + for (int i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&] + { + std::lock_guard lock(recvMsgMutex); + return static_cast(pubCount) == recvMsgs0.size() && + static_cast(pubCount) == recvMsgs1.size(); + }); + + EXPECT_EQ(static_cast(pubCount), recvMsgs0.size()); + EXPECT_EQ(static_cast(pubCount), recvMsgs1.size()); + + // The plugin has two outputs. We expect 10 messages in each output topic + EXPECT_EQ(pubCount, std::count(recvMsgs0.begin(), recvMsgs0.end(), true)); + EXPECT_EQ(pubCount, std::count(recvMsgs1.begin(), recvMsgs1.end(), true)); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with incorrect request type or reply +/// type specified in test/worlds/triggered_publisher.sdf. `InvalidReqType` and +/// `InvalidRepType` do not exist. Hence, the callback will never be invoked and +/// the recvCount will be 0. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(WrongRequestOrResponseType)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_16"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-test"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return recvCount == 0u;}); + EXPECT_EQ(0u, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with different request (Boolean) and +/// reply type (StringMsg). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(BooleanReqStringMsgRep)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_18"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), true); + if (req.data() == true) + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-diff-type-0"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(pubCount, recvCount); +} + +///////////////////////////////////////////////// +/// Test for triggering a service call with different request (StringMsg) and +/// reply type (Boolean). Check `InputMessagesTriggerServices` test for more +/// details on how the test works. This test is very similar except that it has +/// different request and reply type. +TEST_F(TriggeredPublisherTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(StringMsgReqBooleanRep)) +{ + transport::Node node; + auto inputPub = node.Advertise("/in_19"); + std::atomic recvCount{0}; + + auto srvEchoCb = std::function( + [&recvCount](const auto &req, auto &) + { + EXPECT_EQ(req.data(), "test"); + if (req.data() == "test") + { + ++recvCount; + return true; + } + return false; + }); + + // Advertise a dummy service + std::string service = "/srv-diff-type-1"; + node.Advertise(service, srvEchoCb); + + const std::size_t pubCount{10}; + for (std::size_t i = 0; i < pubCount; ++i) + { + EXPECT_TRUE(inputPub.Publish(msgs::Empty())); + IGN_SLEEP_MS(100); + } + + waitUntil(5000, [&]{return pubCount == recvCount;}); + EXPECT_EQ(recvCount, recvCount); +} diff --git a/test/worlds/triggered_publisher.sdf b/test/worlds/triggered_publisher.sdf index 7c9f21c568..2ce7ab2394 100644 --- a/test/worlds/triggered_publisher.sdf +++ b/test/worlds/triggered_publisher.sdf @@ -5,19 +5,25 @@ 0 - + - + data: true - + data: false @@ -27,7 +33,9 @@ - + data: true @@ -36,7 +44,9 @@ - + data: 0 @@ -45,7 +55,9 @@ - + data: 0 @@ -54,7 +66,9 @@ - + data: -5 @@ -75,7 +89,9 @@ - + 1.0 2.0 @@ -83,7 +99,9 @@ - + 1.0 2.0 @@ -91,7 +109,9 @@ - + { @@ -102,7 +122,9 @@ - + { @@ -120,53 +142,152 @@ - + data: 0, data: 1 - + data: 0.5 - + 0.5 - + "value1" - + 1000 - - + + data: 0, data: 1 - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index d07c74c318..ff4759c3ef 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -1,17 +1,20 @@ \page triggeredpublisher Triggered Publisher The `TriggeredPublisher` system publishes a user specified message on an output -topic in response to an input message that matches user specified criteria. The -system works by checking the input against a set of Matchers. Matchers -contain string representations of protobuf messages which are compared for -equality or containment with the input message. Matchers can match the whole -input message or only a specific field inside the message. +topic in response to an input message that matches user specified criteria. It +can also call a user specified service in response to an input +message. The system works by checking the input against a set of Matchers. +Matchers contain string representations of protobuf messages which are compared +for equality or containment with the input message. Matchers can match the +whole input message or only a specific field inside the message. + This tutorial describes how the Triggered Publisher system can be used to cause a box to fall from its initial position by detaching a detachable joint in response to the motion of a vehicle. The tutorial also covers how Triggered Publisher systems can be chained together by showing how the falling of the box -can trigger another box to fall. The finished world SDFormat file for this +can trigger another box to fall. Last, it covers how a service call can be +triggered to reset the robot pose. The finished world SDFormat file for this tutorial can be found in [examples/worlds/triggered_publisher.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo2/examples/worlds/triggered_publisher.sdf) @@ -263,3 +266,27 @@ and publish the start message ``` ign topic -t "/start" -m ignition.msgs.Empty -p " " ``` + +Once both boxes have fallen, we can publish a message to invoke a service call +to reset the robot position as well as set the speed to 0. As shown below, the +`` sets the linear x speed to 0, and the `` tag contains +metadata to invoke a service call to `/world/triggered_publisher/set_pose`. The +`reqMsg` is expressed in the human-readable form of Google Protobuf meesages. +Multiple `` tags can be used as well as with the `` tag. + +```xml + + + + linear: {x: 0} + + + + +``` From 35ce83afed1a7e26e9f6929f8efd4c35eba01cb8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 9 Sep 2022 06:30:23 -0700 Subject: [PATCH 09/10] Fix loading render engine plugins in GUI (#1694) Signed-off-by: Ian Chen Signed-off-by: Silvio --- include/ignition/gazebo/rendering/RenderUtil.hh | 4 ++++ src/gui/plugins/scene_manager/GzSceneManager.cc | 9 +++++++++ src/rendering/RenderUtil.cc | 12 +++++++++--- 3 files changed, 22 insertions(+), 3 deletions(-) diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index a757ec5378..444d9c0daa 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -201,6 +201,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Clears the set of selected entities and lowlights all of them. public: void DeselectAllEntities(); + /// \brief Init render engine plugins paths. This lets gz-rendering know + /// paths to find render engine plugins + public: void InitRenderEnginePluginPaths(); + /// \brief Helper function to get all child links of a model entity. /// \param[in] _entity Entity to find child links /// \return Vector of child links found for the parent entity diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index f04c61d07a..9d0c6b1338 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -59,6 +59,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Rendering utility public: RenderUtil renderUtil; + /// \brief True if render engine plugins paths are initialized + public: bool renderEnginePluginPathsInit{false}; + /// \brief List of new entities from a gui event public: std::set newEntities; @@ -123,6 +126,12 @@ void GzSceneManager::Update(const UpdateInfo &_info, IGN_PROFILE("GzSceneManager::Update"); + if (!this->dataPtr->renderEnginePluginPathsInit) + { + this->dataPtr->renderUtil.InitRenderEnginePluginPaths(); + this->dataPtr->renderEnginePluginPathsInit = true; + } + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 415908ff9c..6f4ef07941 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -2496,6 +2496,14 @@ bool RenderUtil::HeadlessRendering() const return this->dataPtr->isHeadlessRendering; } +///////////////////////////////////////////////// +void RenderUtil::InitRenderEnginePluginPaths() +{ + ignition::common::SystemPaths pluginPath; + pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); + rendering::setPluginPaths(pluginPath.PluginPaths()); +} + ///////////////////////////////////////////////// void RenderUtil::Init() { @@ -2503,9 +2511,7 @@ void RenderUtil::Init() if (nullptr != this->dataPtr->scene) return; - ignition::common::SystemPaths pluginPath; - pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); - rendering::setPluginPaths(pluginPath.PluginPaths()); + this->InitRenderEnginePluginPaths(); std::map params; if (this->dataPtr->useCurrentGLContext) From bacff19cdfa4f5cb8762cbb21cf7eaf32eb64711 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 19 Oct 2022 23:22:34 +0200 Subject: [PATCH 10/10] Fix cmdmodel6.rb and cmdgazebo6.rb contining the same code Signed-off-by: Silvio --- src/cmd/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cmd/CMakeLists.txt b/src/cmd/CMakeLists.txt index e81a98be44..f19fd0d6af 100644 --- a/src/cmd/CMakeLists.txt +++ b/src/cmd/CMakeLists.txt @@ -49,7 +49,7 @@ install( FILES # Note that the major version of the library is included in the name. set(cmd_model_script_name "cmdmodel${PROJECT_VERSION_MAJOR}.rb") set(cmd_model_script_generated "${CMAKE_CURRENT_BINARY_DIR}/$_${cmd_model_script_name}") -set(cmd_model_script_configured "${CMAKE_CURRENT_BINARY_DIR}/${cmd_script_name}.configured") +set(cmd_model_script_configured "${CMAKE_CURRENT_BINARY_DIR}/${cmd_model_script_name}.configured") configure_file( "cmdmodel.rb.in"