diff --git a/CMakeLists.txt b/CMakeLists.txt index ce24763e77..4d4f036811 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,7 +75,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-gui -ign_find_package(ignition-gui4 REQUIRED VERSION 4.1) +ign_find_package(ignition-gui4 REQUIRED VERSION 4.1.1) set(IGN_GUI_VER ${ignition-gui4_VERSION_MAJOR}) ign_find_package (Qt5 COMPONENTS diff --git a/examples/worlds/spaces.sdf b/examples/worlds/spaces.sdf new file mode 100644 index 0000000000..0109d50943 --- /dev/null +++ b/examples/worlds/spaces.sdf @@ -0,0 +1,108 @@ + + + + + + 0.001 + 1.0 + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + 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 + + + + + + + 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 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 39d790f7f9..3e9b9c8fec 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -139,6 +139,21 @@ namespace ignition const Entity &_entity, const EntityComponentManager &_ecm); + /// \brief Helper function to generate a valid transport topic, given + /// a list of topics ordered by preference. The generated topic will be, + /// in this order: + /// + /// 1. The first topic unchanged, if valid. + /// 2. A valid version of the first topic, if possible. + /// 3. The second topic unchanged, if valid. + /// 4. A valid version of the second topic, if possible. + /// 5. ... + /// 6. If no valid topics could be generated, return an empty string. + /// + /// \param[in] _topics Topics ordered by preference. + std::string IGNITION_GAZEBO_VISIBLE validTopic( + const std::vector &_topics); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; diff --git a/src/LevelManager.cc b/src/LevelManager.cc index d3dcbec7d0..1b54f01848 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -76,8 +76,14 @@ LevelManager::LevelManager(SimulationRunner *_runner, const bool _useLevels) this->ReadLevelPerformerInfo(); this->CreatePerformers(); - std::string service = "/world/"; - service += this->runner->sdfWorld->Name() + "/level/set_performer"; + std::string service = transport::TopicUtils::AsValidTopic("/world/" + + this->runner->sdfWorld->Name() + "/level/set_performer"); + if (service.empty()) + { + ignerr << "Failed to generate set_performer topic for world [" + << this->runner->sdfWorld->Name() << "]" << std::endl; + return; + } this->node.Advertise(service, &LevelManager::OnSetPerformer, this); } diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 9777417c33..e047ffaa39 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -157,15 +157,20 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // World control transport::NodeOptions opts; + std::string ns{"/world/" + this->worldName}; if (this->networkMgr) { - opts.SetNameSpace(this->networkMgr->Namespace() + - "/world/" + this->worldName); + ns = this->networkMgr->Namespace() + ns; } - else + + auto validNs = transport::TopicUtils::AsValidTopic(ns); + if (validNs.empty()) { - opts.SetNameSpace("/world/" + this->worldName); + ignerr << "Invalid namespace [" << ns + << "], not initializing runner transport." << std::endl; + return; } + opts.SetNameSpace(validNs); this->node = std::make_unique(opts); diff --git a/src/Util.cc b/src/Util.cc index 5b5995ae9e..828483d0f0 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -24,6 +24,7 @@ #endif #include #include +#include #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" @@ -416,6 +417,27 @@ ignition::gazebo::Entity topLevelModel(const Entity &_entity, } return entity; } + +////////////////////////////////////////////////// +std::string validTopic(const std::vector &_topics) +{ + for (const auto &topic : _topics) + { + auto validTopic = transport::TopicUtils::AsValidTopic(topic); + if (validTopic.empty()) + { + ignerr << "Topic [" << topic << "] is invalid, ignoring." << std::endl; + continue; + } + if (validTopic != topic) + { + igndbg << "Topic [" << topic << "] changed to valid topic [" + << validTopic << "]" << std::endl; + } + return validTopic; + } + return std::string(); +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 501fdb5939..d699b3dc3b 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -16,6 +16,7 @@ */ #include +#include #include #include @@ -36,8 +37,18 @@ using namespace ignition; using namespace gazebo; +/// \brief Tests for Util.hh +class UtilTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + } +}; + ///////////////////////////////////////////////// -TEST(UtilTest, ScopedName) +TEST_F(UtilTest, ScopedName) { EntityComponentManager ecm; @@ -219,7 +230,7 @@ TEST(UtilTest, ScopedName) } ///////////////////////////////////////////////// -TEST(UtilTest, EntityTypeId) +TEST_F(UtilTest, EntityTypeId) { EntityComponentManager ecm; @@ -264,7 +275,7 @@ TEST(UtilTest, EntityTypeId) } ///////////////////////////////////////////////// -TEST(UtilTest, EntityTypeStr) +TEST_F(UtilTest, EntityTypeStr) { EntityComponentManager ecm; @@ -309,7 +320,7 @@ TEST(UtilTest, EntityTypeStr) } ///////////////////////////////////////////////// -TEST(UtilTest, RemoveParentScopedName) +TEST_F(UtilTest, RemoveParentScopedName) { EXPECT_EQ(removeParentScope("world/world_name", "/"), "world_name"); EXPECT_EQ(removeParentScope("world::world_name::light::lightA_name", "::"), @@ -324,7 +335,7 @@ TEST(UtilTest, RemoveParentScopedName) } ///////////////////////////////////////////////// -TEST(UtilTest, AsFullPath) +TEST_F(UtilTest, AsFullPath) { const std::string relativeUriUnix{"meshes/collision.dae"}; const std::string relativeUriWindows{"meshes\\collision.dae"}; @@ -408,7 +419,7 @@ TEST(UtilTest, AsFullPath) } ///////////////////////////////////////////////// -TEST(UtilTest, TopLevelModel) +TEST_F(UtilTest, TopLevelModel) { EntityComponentManager ecm; @@ -463,3 +474,27 @@ TEST(UtilTest, TopLevelModel) // model C should have itself as the top level entity EXPECT_EQ(modelCEntity, topLevelModel(modelCEntity, ecm)); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, ValidTopic) +{ + std::string good{"good"}; + std::string fixable{"not bad~"}; + std::string invalid{"@~@~@~"}; + + EXPECT_EQ("good", validTopic({good})); + EXPECT_EQ("not_bad", validTopic({fixable})); + EXPECT_EQ("", validTopic({invalid})); + + EXPECT_EQ("good", validTopic({good, fixable})); + EXPECT_EQ("not_bad", validTopic({fixable, good})); + + EXPECT_EQ("good", validTopic({good, invalid})); + EXPECT_EQ("good", validTopic({invalid, good})); + + EXPECT_EQ("not_bad", validTopic({fixable, invalid})); + EXPECT_EQ("not_bad", validTopic({invalid, fixable})); + + EXPECT_EQ("not_bad", validTopic({fixable, invalid, good})); + EXPECT_EQ("good", validTopic({invalid, good, fixable})); +} diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index b375e912fe..1c4912a6d2 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -194,18 +194,31 @@ std::unique_ptr createGui( // Request GUI info for each world result = false; - service = std::string("/world/" + worldName + "/gui/info"); - - igndbg << "Requesting GUI from [" << service << "]..." << std::endl; - - // Request and block ignition::msgs::GUI res; - executed = node.Request(service, timeout, res, result); + service = transport::TopicUtils::AsValidTopic("/world/" + worldName + + "/gui/info"); + if (service.empty()) + { + ignerr << "Failed to generate valid service for world [" << worldName + << "]" << std::endl; + } + else + { + igndbg << "Requesting GUI from [" << service << "]..." << std::endl; - if (!executed) - ignerr << "Service call timed out for [" << service << "]" << std::endl; - else if (!result) - ignerr << "Service call failed for [" << service << "]" << std::endl; + // Request and block + executed = node.Request(service, timeout, res, result); + + if (!executed) + { + ignerr << "Service call timed out for [" << service << "]" + << std::endl; + } + else if (!result) + { + ignerr << "Service call failed for [" << service << "]" << std::endl; + } + } // GUI runner auto runner = new ignition::gazebo::GuiRunner(worldName); diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 565838a06c..6f88a441fe 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -40,7 +40,14 @@ GuiRunner::GuiRunner(const std::string &_worldName) winWorldNames.append(QString::fromStdString(_worldName)); win->setProperty("worldNames", winWorldNames); - this->stateTopic = "/world/" + _worldName + "/state"; + this->stateTopic = transport::TopicUtils::AsValidTopic("/world/" + + _worldName + "/state"); + if (this->stateTopic.empty()) + { + ignerr << "Failed to generate valid topic for world [" << _worldName << "]" + << std::endl; + return; + } common::addFindFileURICallback([] (common::URI _uri) { @@ -63,6 +70,15 @@ void GuiRunner::RequestState() std::string id = std::to_string(gui::App()->applicationPid()); std::string reqSrv = this->node.Options().NameSpace() + "/" + id + "/state_async"; + auto reqSrvValid = transport::TopicUtils::AsValidTopic(reqSrv); + if (reqSrvValid.empty()) + { + ignerr << "Failed to generate valid service [" << reqSrv << "]" + << std::endl; + return; + } + reqSrv = reqSrvValid; + this->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this); ignition::msgs::StringMsg req; req.set_data(reqSrv); diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 2e51b6f440..a91f4821c4 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -1029,6 +1029,15 @@ void IgnRenderer::HandleModelPlacement() this->dataPtr->createCmdService = "/world/" + this->worldName + "/create"; } + this->dataPtr->createCmdService = transport::TopicUtils::AsValidTopic( + this->dataPtr->createCmdService); + if (this->dataPtr->createCmdService.empty()) + { + ignerr << "Failed to create valid create command service for world [" + << this->worldName <<"]" << std::endl; + return; + } + this->dataPtr->node.Request(this->dataPtr->createCmdService, req, cb); this->dataPtr->isPlacing = false; this->dataPtr->mouseDirty = false; @@ -1247,6 +1256,14 @@ void IgnRenderer::HandleMouseTransformControl() this->dataPtr->poseCmdService = "/world/" + this->worldName + "/set_pose"; } + this->dataPtr->poseCmdService = transport::TopicUtils::AsValidTopic( + this->dataPtr->poseCmdService); + if (this->dataPtr->poseCmdService.empty()) + { + ignerr << "Failed to create valid pose command service for world [" + << this->worldName <<"]" << std::endl; + return; + } this->dataPtr->node.Request(this->dataPtr->poseCmdService, req, cb); } diff --git a/src/systems/apply_joint_force/ApplyJointForce.cc b/src/systems/apply_joint_force/ApplyJointForce.cc index 582d46e101..042f585565 100644 --- a/src/systems/apply_joint_force/ApplyJointForce.cc +++ b/src/systems/apply_joint_force/ApplyJointForce.cc @@ -25,6 +25,7 @@ #include "ignition/gazebo/components/JointForceCmd.hh" #include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" using namespace ignition; using namespace gazebo; @@ -93,8 +94,15 @@ void ApplyJointForce::Configure(const Entity &_entity, } // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + - this->dataPtr->jointName + "/cmd_force"}; + auto topic = transport::TopicUtils::AsValidTopic("/model/" + + this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + + "/cmd_force"); + if (topic.empty()) + { + ignerr << "Failed to create valid topic for [" << this->dataPtr->jointName + << "]" << std::endl; + return; + } this->dataPtr->node.Subscribe(topic, &ApplyJointForcePrivate::OnCmdForce, this->dataPtr.get()); diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 47530eff67..8c8f94b32d 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -297,16 +297,28 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, "/battery/" + _sdf->Get("battery_name") + "/recharge/stop"; - this->dataPtr->node.Advertise(enableRechargeTopic, + auto validEnableRechargeTopic = transport::TopicUtils::AsValidTopic( + enableRechargeTopic); + auto validDisableRechargeTopic = transport::TopicUtils::AsValidTopic( + disableRechargeTopic); + if (validEnableRechargeTopic.empty() || validDisableRechargeTopic.empty()) + { + ignerr << "Failed to create valid topics. Not valid: [" + << enableRechargeTopic << "] and [" << disableRechargeTopic + << "]" << std::endl; + return; + } + + this->dataPtr->node.Advertise(validEnableRechargeTopic, &LinearBatteryPluginPrivate::OnEnableRecharge, this->dataPtr.get()); - this->dataPtr->node.Advertise(disableRechargeTopic, + this->dataPtr->node.Advertise(validDisableRechargeTopic, &LinearBatteryPluginPrivate::OnDisableRecharge, this->dataPtr.get()); if (_sdf->HasElement("recharge_by_topic")) { - this->dataPtr->node.Subscribe(enableRechargeTopic, + this->dataPtr->node.Subscribe(validEnableRechargeTopic, &LinearBatteryPluginPrivate::OnEnableRecharge, this->dataPtr.get()); - this->dataPtr->node.Subscribe(disableRechargeTopic, + this->dataPtr->node.Subscribe(validDisableRechargeTopic, &LinearBatteryPluginPrivate::OnDisableRecharge, this->dataPtr.get()); } } @@ -338,10 +350,19 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, // Setup battery state topic std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + "/battery/" + this->dataPtr->battery->Name() + "/state"}; + + auto validStateTopic = transport::TopicUtils::AsValidTopic(stateTopic); + if (validStateTopic.empty()) + { + ignerr << "Failed to create valid state topic [" + << stateTopic << "]" << std::endl; + return; + } + transport::AdvertiseMessageOptions opts; opts.SetMsgsPerSec(50); this->dataPtr->statePub = this->dataPtr->node.Advertise( - stateTopic, opts); + validStateTopic, opts); } ///////////////////////////////////////////////// diff --git a/src/systems/breadcrumbs/Breadcrumbs.cc b/src/systems/breadcrumbs/Breadcrumbs.cc index a06789d845..ba98482007 100644 --- a/src/systems/breadcrumbs/Breadcrumbs.cc +++ b/src/systems/breadcrumbs/Breadcrumbs.cc @@ -42,6 +42,7 @@ #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Util.hh" using namespace ignition; using namespace gazebo; @@ -132,11 +133,15 @@ void Breadcrumbs::Configure(const Entity &_entity, } // Subscribe to commands - std::string topic{"/model/" + this->model.Name(_ecm) + "/breadcrumbs/" + - this->modelRoot.ModelByIndex(0)->Name() + "/deploy"}; - + std::vector topics; if (_sdf->HasElement("topic")) - topic = _sdf->Get("topic"); + { + topics.push_back(_sdf->Get("topic")); + } + topics.push_back("/model/" + + this->model.Name(_ecm) + "/breadcrumbs/" + + this->modelRoot.ModelByIndex(0)->Name() + "/deploy"); + auto topic = validTopic(topics); this->node.Subscribe(topic, &Breadcrumbs::OnDeploy, this); this->remainingPub = this->node.Advertise(topic + "/remaining"); diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 52c24ad0f9..cadc7b4b3d 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -200,7 +200,13 @@ void CameraVideoRecorder::Configure( // video recorder service topic name if (_sdf->HasElement("service")) { - this->dataPtr->service = _sdf->Get("service"); + this->dataPtr->service = transport::TopicUtils::AsValidTopic( + _sdf->Get("service")); + if (this->dataPtr->service.empty()) + { + ignerr << "Service [" << _sdf->Get("service") + << "] not valid. Ignoring." << std::endl; + } } this->dataPtr->eventMgr = &_eventMgr; @@ -208,7 +214,15 @@ void CameraVideoRecorder::Configure( sdf::Sensor sensorSdf = cameraEntComp->Data(); std::string topic = sensorSdf.Topic(); if (topic.empty()) - topic = scopedName(_entity, _ecm) + "/image"; + { + auto scoped = scopedName(_entity, _ecm); + topic = transport::TopicUtils::AsValidTopic(scoped + "/image"); + if (topic.empty()) + { + ignerr << "Failed to generate valid topic for entity [" << scoped + << "]" << std::endl; + } + } this->dataPtr->sensorTopic = topic; } @@ -363,8 +377,15 @@ void CameraVideoRecorder::PostUpdate(const UpdateInfo &, if (this->dataPtr->service.empty()) { - this->dataPtr->service = scopedName(this->dataPtr->entity, _ecm) + - "/record_video"; + auto scoped = scopedName(this->dataPtr->entity, _ecm); + this->dataPtr->service = transport::TopicUtils::AsValidTopic(scoped + + "/record_video"); + if (this->dataPtr->service.empty()) + { + ignerr << "Failed to create valid service for [" << scoped << "]" + << std::endl; + } + return; } this->dataPtr->node.Advertise(this->dataPtr->service, diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 1ed2125a1f..0946aa9505 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include @@ -22,13 +24,14 @@ #include -#include "ignition/gazebo/Model.hh" #include "ignition/gazebo/components/DetachableJoint.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" #include "DetachableJoint.hh" @@ -93,9 +96,14 @@ void DetachableJoint::Configure(const Entity &_entity, } // Setup detach topic - std::string defaultTopic{"/model/" + this->model.Name(_ecm) + - "/detachable_joint/detach"}; - this->topic = _sdf->Get("topic", defaultTopic).first; + std::vector topics; + if (_sdf->HasElement("topic")) + { + topics.push_back(_sdf->Get("topic")); + } + topics.push_back("/model/" + this->model.Name(_ecm) + + "/detachable_joint/detach"); + this->topic = validTopic(topics); this->suppressChildWarning = _sdf->Get("suppress_child_warning", this->suppressChildWarning) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index dfd78b7769..1686beebcd 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -36,6 +36,7 @@ #include "ignition/gazebo/components/JointVelocityCmd.hh" #include "ignition/gazebo/Link.hh" #include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" #include "SpeedLimiter.hh" @@ -253,16 +254,26 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->wheelRadius, this->dataPtr->wheelRadius); // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; + std::vector topics; if (_sdf->HasElement("topic")) - topic = _sdf->Get("topic"); + { + topics.push_back(_sdf->Get("topic")); + } + topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); + auto topic = validTopic(topics); + this->dataPtr->node.Subscribe(topic, &DiffDrivePrivate::OnCmdVel, this->dataPtr.get()); - std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/odometry"}; + std::vector odomTopics; if (_sdf->HasElement("odom_topic")) - odomTopic = _sdf->Get("odom_topic"); + { + odomTopics.push_back(_sdf->Get("odom_topic")); + } + odomTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry"); + auto odomTopic = validTopic(odomTopics); + this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopic); diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 42990d4447..dd2f645f1d 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -131,8 +131,15 @@ void JointController::Configure(const Entity &_entity, } // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" + - this->dataPtr->jointName + "/cmd_vel"}; + std::string topic = transport::TopicUtils::AsValidTopic("/model/" + + this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + + "/cmd_vel"); + if (topic.empty()) + { + ignerr << "Failed to create topic for joint [" << this->dataPtr->jointName + << "]" << std::endl; + return; + } this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, this->dataPtr.get()); diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index c048ff0e23..5eaefaf45e 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -147,9 +147,15 @@ void JointPositionController::Configure(const Entity &_entity, this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset); // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + - "/joint/" + this->dataPtr->jointName + "/" + - std::to_string(this->dataPtr->jointIndex) + "/cmd_pos"}; + std::string topic = transport::TopicUtils::AsValidTopic("/model/" + + this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + + "/" + std::to_string(this->dataPtr->jointIndex) + "/cmd_pos"); + if (topic.empty()) + { + ignerr << "Failed to create topic for joint [" << this->dataPtr->jointName + << "]" << std::endl; + return; + } this->dataPtr->node.Subscribe( topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get()); diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index 233d7049a9..3febe2343a 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -291,11 +291,31 @@ bool LogRecordPrivate::Start(const std::string &_logPath, // Use directory basename as topic name, to be able to retrieve at playback std::string sdfTopic = "/" + common::basename(this->logPath) + "/sdf"; - this->sdfPub = this->node.Advertise(sdfTopic, this->sdfMsg.GetTypeName()); + auto validSdfTopic = transport::TopicUtils::AsValidTopic(sdfTopic); + if (!validSdfTopic.empty()) + { + this->sdfPub = this->node.Advertise(validSdfTopic, + this->sdfMsg.GetTypeName()); + } + else + { + ignerr << "Failed to generate valid topic to publish SDF. Tried [" + << sdfTopic << "]." << std::endl; + } // TODO(louise) Combine with SceneBroadcaster's state topic std::string stateTopic = "/world/" + this->worldName + "/changed_state"; - this->statePub = this->node.Advertise(stateTopic); + auto validStateTopic = transport::TopicUtils::AsValidTopic(stateTopic); + if (!validStateTopic.empty()) + { + this->statePub = this->node.Advertise( + validStateTopic); + } + else + { + ignerr << "Failed to generate valid topic to publish state. Tried [" + << stateTopic << "]." << std::endl; + } // Append file name std::string dbPath = common::joinPaths(this->logPath, "state.tlog"); diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc index 33b9310b2b..78ab1b7d65 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.cc @@ -403,14 +403,21 @@ void LogicalAudioSensorPluginPrivate::CreateAudioSource( }; // create services for this source - const auto full_name = scopedName(entity, _ecm); - if (!this->node.Advertise(full_name + "/play", playSrvCb)) + const auto fullName = scopedName(entity, _ecm); + auto validName = transport::TopicUtils::AsValidTopic(fullName); + if (validName.empty()) + { + ignerr << "Failed to create valid topics with entity scoped name [" + << fullName << "]" << std::endl; + return; + } + if (!this->node.Advertise(validName + "/play", playSrvCb)) { ignerr << "Error advertising the play source service for source " << id << " in entity " << _parent << ". " << kSourceSkipMsg; return; } - if (!this->node.Advertise(full_name + "/stop", stopSrvCb)) + if (!this->node.Advertise(validName + "/stop", stopSrvCb)) { ignerr << "Error advertising the stop source service for source " << id << " in entity " << _parent << ". " << kSourceSkipMsg; diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.cc b/src/systems/multicopter_control/MulticopterVelocityControl.cc index edeed38318..9eb4b248b4 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.cc +++ b/src/systems/multicopter_control/MulticopterVelocityControl.cc @@ -259,8 +259,15 @@ void MulticopterVelocityControl::Configure(const Entity &_entity, if (sdfClone->HasElement("robotNamespace")) { - this->robotNamespace = - sdfClone->Get("robotNamespace"); + this->robotNamespace = transport::TopicUtils::AsValidTopic( + sdfClone->Get("robotNamespace")); + if (this->robotNamespace.empty()) + { + ignerr << "Robot namespace [" + << sdfClone->Get("robotNamespace") <<"] is invalid." + << std::endl; + return; + } } else { @@ -270,9 +277,23 @@ void MulticopterVelocityControl::Configure(const Entity &_entity, sdfClone->Get("commandSubTopic", this->commandSubTopic, this->commandSubTopic); + this->commandSubTopic = transport::TopicUtils::AsValidTopic( + this->commandSubTopic); + if (this->commandSubTopic.empty()) + { + ignerr << "Invalid command sub-topic." << std::endl; + return; + } sdfClone->Get("enableSubTopic", this->enableSubTopic, this->enableSubTopic); + this->enableSubTopic = transport::TopicUtils::AsValidTopic( + this->enableSubTopic); + if (this->enableSubTopic.empty()) + { + ignerr << "Invalid enable sub-topic." << std::endl; + return; + } // Subscribe to actuator command messages std::string topic{this->robotNamespace + "/" + this->commandSubTopic}; diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index 730103d407..dbb5102d00 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -357,8 +357,14 @@ void MulticopterMotorModel::Configure(const Entity &_entity, this->dataPtr->refMotorInput); // Subscribe to actuator command messages - std::string topic{this->dataPtr->robotNamespace + "/" - + this->dataPtr->commandSubTopic}; + std::string topic = transport::TopicUtils::AsValidTopic( + this->dataPtr->robotNamespace + "/" + this->dataPtr->commandSubTopic); + if (topic.empty()) + { + ignerr << "Failed to create topic for [" << this->dataPtr->robotNamespace + << "]" << std::endl; + return; + } this->dataPtr->node.Subscribe(topic, &MulticopterMotorModelPrivate::OnActuatorMsg, this->dataPtr.get()); } diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 576a081bc0..0918c756bb 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -444,8 +444,16 @@ void SceneBroadcasterPrivate::PoseUpdate(const UpdateInfo &_info, ////////////////////////////////////////////////// void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) { + auto ns = transport::TopicUtils::AsValidTopic("/world/" + _worldName); + if (ns.empty()) + { + ignerr << "Failed to create valid namespace for world [" << _worldName + << "]" << std::endl; + return; + } + transport::NodeOptions opts; - opts.SetNameSpace("/world/" + _worldName); + opts.SetNameSpace(ns); this->node = std::make_unique(opts); // Scene info service @@ -487,7 +495,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) << stateAsyncService << "]" << std::endl; // Scene info topic - std::string sceneTopic{"/world/" + _worldName + "/scene/info"}; + std::string sceneTopic{ns + "/scene/info"}; this->scenePub = this->node->Advertise(sceneTopic); @@ -495,7 +503,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) << "]" << std::endl; // Entity deletion publisher - std::string deletionTopic{"/world/" + _worldName + "/scene/deletion"}; + std::string deletionTopic{ns + "/scene/deletion"}; this->deletionPub = this->node->Advertise(deletionTopic); @@ -504,7 +512,7 @@ void SceneBroadcasterPrivate::SetupTransport(const std::string &_worldName) << std::endl; // State topic - std::string stateTopic{"/world/" + _worldName + "/state"}; + std::string stateTopic{ns + "/state"}; this->statePub = this->node->Advertise(stateTopic); diff --git a/src/systems/touch_plugin/TouchPlugin.cc b/src/systems/touch_plugin/TouchPlugin.cc index daa1098aff..ccd177bb90 100644 --- a/src/systems/touch_plugin/TouchPlugin.cc +++ b/src/systems/touch_plugin/TouchPlugin.cc @@ -163,7 +163,14 @@ void TouchPluginPrivate::Load(const EntityComponentManager &_ecm, ignerr << "Missing required parameter " << std::endl; return; } - this->ns = _sdf->Get("namespace"); + this->ns = transport::TopicUtils::AsValidTopic(_sdf->Get( + "namespace")); + if (this->ns.empty()) + { + ignerr << " [" << _sdf->Get("namespace") + << "] is invalid." << std::endl; + return; + } // Target time if (!_sdf->HasElement("time")) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index ddbcb58de7..8fd37edba8 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -486,10 +486,11 @@ void TriggeredPublisher::Configure(const Entity &, return; } - this->inputTopic = inputElem->Get("topic"); + auto inTopic = inputElem->Get("topic"); + this->inputTopic = transport::TopicUtils::AsValidTopic(inTopic); if (this->inputTopic.empty()) { - ignerr << "Input topic cannot be empty\n"; + ignerr << "Invalid input topic [" << inTopic << "]" << std::endl; return; } @@ -538,10 +539,11 @@ void TriggeredPublisher::Configure(const Entity &, ignerr << "Output message type cannot be empty\n"; continue; } - info.topic = outputElem->Get("topic"); + auto topic = outputElem->Get("topic"); + info.topic = transport::TopicUtils::AsValidTopic(topic); if (info.topic.empty()) { - ignerr << "Output topic cannot be empty\n"; + ignerr << "Invalid topic [" << topic << "]" << std::endl; continue; } const std::string msgStr = outputElem->Get(); diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 25e1a3ec15..12aa08671a 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -223,27 +223,37 @@ void UserCommands::Configure(const Entity &_entity, const components::Name *constCmp = _ecm.Component(_entity); const std::string &worldName = constCmp->Data(); + auto validWorldName = transport::TopicUtils::AsValidTopic(worldName); + if (validWorldName.empty()) + { + ignerr << "World name [" << worldName + << "] doesn't work well with transport, services not advertised." + << std::endl; + return; + } + // Create service - std::string createService{"/world/" + worldName + "/create"}; + std::string createService{"/world/" + validWorldName + "/create"}; this->dataPtr->node.Advertise(createService, &UserCommandsPrivate::CreateService, this->dataPtr.get()); // Create service for EntityFactory_V - std::string createServiceMultiple{"/world/" + worldName + "/create_multiple"}; + std::string createServiceMultiple{"/world/" + validWorldName + + "/create_multiple"}; this->dataPtr->node.Advertise(createServiceMultiple, &UserCommandsPrivate::CreateServiceMultiple, this->dataPtr.get()); ignmsg << "Create service on [" << createService << "]" << std::endl; // Remove service - std::string removeService{"/world/" + worldName + "/remove"}; + std::string removeService{"/world/" + validWorldName + "/remove"}; this->dataPtr->node.Advertise(removeService, &UserCommandsPrivate::RemoveService, this->dataPtr.get()); ignmsg << "Remove service on [" << removeService << "]" << std::endl; // Pose service - std::string poseService{"/world/" + worldName + "/set_pose"}; + std::string poseService{"/world/" + validWorldName + "/set_pose"}; this->dataPtr->node.Advertise(poseService, &UserCommandsPrivate::PoseService, this->dataPtr.get()); diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index d6e07f48e7..78b1913d67 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -26,6 +27,7 @@ #include "ignition/gazebo/components/AngularVelocityCmd.hh" #include "ignition/gazebo/components/LinearVelocityCmd.hh" #include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" #include "VelocityControl.hh" @@ -87,9 +89,13 @@ void VelocityControl::Configure(const Entity &_entity, } // Subscribe to commands - std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"}; + std::vector topics; if (_sdf->HasElement("topic")) - topic = _sdf->Get("topic"); + { + topics.push_back(_sdf->Get("topic")); + } + topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); + auto topic = validTopic(topics); this->dataPtr->node.Subscribe( topic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); diff --git a/src/systems/wind_effects/WindEffects.cc b/src/systems/wind_effects/WindEffects.cc index 11ba62836d..76a70ea0b3 100644 --- a/src/systems/wind_effects/WindEffects.cc +++ b/src/systems/wind_effects/WindEffects.cc @@ -305,12 +305,20 @@ void WindEffectsPrivate::Load(EntityComponentManager &_ecm, ////////////////////////////////////////////////// void WindEffectsPrivate::SetupTransport(const std::string &_worldName) { + auto validWorldName = transport::TopicUtils::AsValidTopic(_worldName); + if (validWorldName.empty()) + { + ignerr << "Failed to setup transport, invalid world name [" << _worldName + << "]" << std::endl; + return; + } + // Wind seed velocity topic - this->node.Subscribe("/world/" + _worldName + "/wind", + this->node.Subscribe("/world/" + validWorldName + "/wind", &WindEffectsPrivate::OnWindMsg, this); // Wind info service - this->node.Advertise("/world/" + _worldName + "/wind_info", + this->node.Advertise("/world/" + validWorldName + "/wind_info", &WindEffectsPrivate::WindInfoService, this); }