From 74c41ed0707749f26a264814a8498090aa8be698 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 30 Aug 2022 17:01:52 +0200 Subject: [PATCH 1/3] Add topic parameter. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/thruster/Thruster.cc | 21 +++++++++++++++++++-- src/systems/thruster/Thruster.hh | 4 +++- 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 92a74bb3a7..b9e49b33c1 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 = ignition::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 = ignition::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 From 45dfc8324944641418aa28d8a84ebdf90c644417 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Tue, 30 Aug 2022 17:22:22 +0200 Subject: [PATCH 2/3] Test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- test/integration/thruster.cc | 26 +++++++++++++++++--------- test/worlds/thruster_battery.sdf | 1 + 2 files changed, 18 insertions(+), 9 deletions(-) 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 22:04:04 +0200 Subject: [PATCH 3/3] Remove ignition:: MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- src/systems/thruster/Thruster.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index b9e49b33c1..b775cf26e4 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -177,7 +177,7 @@ void Thruster::Configure( // Get a custom topic. if (_sdf->HasElement("topic")) { - this->dataPtr->topic = ignition::transport::TopicUtils::AsValidTopic( + this->dataPtr->topic = transport::TopicUtils::AsValidTopic( _sdf->Get("topic")); } @@ -215,7 +215,7 @@ void Thruster::Configure( if (!this->dataPtr->topic.empty()) thrusterTopic = ns + "/" + this->dataPtr->topic; - thrusterTopic = ignition::transport::TopicUtils::AsValidTopic(thrusterTopic); + thrusterTopic = transport::TopicUtils::AsValidTopic(thrusterTopic); this->dataPtr->node.Subscribe( thrusterTopic,