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