From ac1089278a063cfddc9b1322d1abcbb1ce760c00 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Tue, 9 Feb 2021 09:48:45 -0800
Subject: [PATCH] Make topics configurable for joint controllers (#584)

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 examples/worlds/joint_position_controller.sdf              | 7 ++++---
 src/systems/joint_controller/JointController.cc            | 4 ++++
 .../joint_position_controller/JointPositionController.cc   | 4 ++++
 3 files changed, 12 insertions(+), 3 deletions(-)

diff --git a/examples/worlds/joint_position_controller.sdf b/examples/worlds/joint_position_controller.sdf
index 6d2ad62e83..b2bd104509 100644
--- a/examples/worlds/joint_position_controller.sdf
+++ b/examples/worlds/joint_position_controller.sdf
@@ -4,9 +4,9 @@
 
   Try sending joint position commands:
 
-    ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: -1.0"
+    ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: -1.0"
 
-    ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: 1.0"
+    ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: 1.0"
 -->
 <sdf version="1.6">
   <world name="default">
@@ -45,7 +45,7 @@
           <property type="double" key="height">72</property>
           <property type="double" key="width">121</property>
           <property type="double" key="z">1</property>
-          
+
           <property type="string" key="state">floating</property>
           <anchors target="3D View">
             <line own="left" target="left"/>
@@ -156,6 +156,7 @@
         filename="ignition-gazebo-joint-position-controller-system"
         name="ignition::gazebo::systems::JointPositionController">
         <joint_name>j1</joint_name>
+        <topic>rotor_cmd</topic>
         <p_gain>1</p_gain>
         <i_gain>0.1</i_gain>
         <d_gain>0.01</d_gain>
diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc
index 42990d4447..695ee62d6d 100644
--- a/src/systems/joint_controller/JointController.cc
+++ b/src/systems/joint_controller/JointController.cc
@@ -133,6 +133,10 @@ void JointController::Configure(const Entity &_entity,
   // Subscribe to commands
   std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) + "/joint/" +
                     this->dataPtr->jointName + "/cmd_vel"};
+  if (_sdf->HasElement("topic"))
+  {
+    topic = _sdf->Get<std::string>("topic");
+  }
   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..691336d901 100644
--- a/src/systems/joint_position_controller/JointPositionController.cc
+++ b/src/systems/joint_position_controller/JointPositionController.cc
@@ -150,6 +150,10 @@ void JointPositionController::Configure(const Entity &_entity,
   std::string topic{"/model/" + this->dataPtr->model.Name(_ecm) +
                     "/joint/" + this->dataPtr->jointName + "/" +
                     std::to_string(this->dataPtr->jointIndex) + "/cmd_pos"};
+  if (_sdf->HasElement("topic"))
+  {
+    topic = _sdf->Get<std::string>("topic");
+  }
   this->dataPtr->node.Subscribe(
       topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get());