Add topic parameter to thrust plugin (#1681)

* Add topic parameter.

Signed-off-by: Carlos Agüero <[email protected]>

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<double>("fluid_density");
   }
 
+  // Get a custom topic.
+  if (_sdf->HasElement("topic"))
+  {
+    this->dataPtr->topic = transport::TopicUtils::AsValidTopic(
+      _sdf->Get<std::string>("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,

src/systems/thruster/Thruster.hh

@@ -40,8 +40,10 @@ namespace systems
   ///
   /// ## System Parameters
   /// - <namespace> - 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]
+  /// - <topic> - The topic for receiving thrust commands. [Optional]
   /// - <joint_name> - This is the joint in the model which corresponds to the
   ///   propeller. [Required]
   /// - <fluid_density> - The fluid density of the liquid in which the thruster

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<msgs::Double>(
-      "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust");
+  auto pub = node.Advertise<msgs::Double>(_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);
 }
 

test/worlds/thruster_battery.sdf

@@ -105,6 +105,7 @@
         <velocity_control>true</velocity_control>
         <max_thrust_cmd>300</max_thrust_cmd>
         <min_thrust_cmd>0</min_thrust_cmd>
+        <topic>thrust</topic>
       </plugin>
 
       <plugin filename="ignition-gazebo-linearbatteryplugin-system"