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:/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"