From adadda7873895a53f4919f75db429d909d1f0b17 Mon Sep 17 00:00:00 2001
From: Jasmeet Singh <jasmeet0915@gmail.com>
Date: Tue, 16 Nov 2021 03:54:26 +0530
Subject: [PATCH] Configurable joint state publisher's topic (#1076)

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>
---
 .../JointStatePublisher.cc                    | 32 ++++++++++++++++---
 .../JointStatePublisher.hh                    |  4 +++
 2 files changed, 32 insertions(+), 4 deletions(-)

diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc
index 9830edfbd7..f19b1b155e 100644
--- a/src/systems/joint_state_publisher/JointStatePublisher.cc
+++ b/src/systems/joint_state_publisher/JointStatePublisher.cc
@@ -31,6 +31,7 @@
 #include "ignition/gazebo/components/JointVelocity.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/Util.hh"
 
 using namespace ignition;
 using namespace gazebo;
@@ -89,6 +90,14 @@ void JointStatePublisher::Configure(
       this->CreateComponents(_ecm, joint);
     }
   }
+
+  // Advertise the state topic
+  // Sets to provided topic if available
+  if (_sdf->HasElement("topic"))
+  {
+    this->topic = _sdf->Get<std::string>("topic");
+  }
+
 }
 
 //////////////////////////////////////////////////
@@ -142,11 +151,26 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info,
       worldName = _ecm.Component<components::Name>(
           parentEntity->Data())->Data();
 
-      // Advertise the state topic
-      std::string topic = std::string("/world/") + worldName + "/model/"
-        + this->model.Name(_ecm) + "/joint_state";
+      // if topic not set it will be empty
+      std::vector<std::string> topics;
+      // this helps avoid unecesarry invalid topic error
+      if (!this->topic.empty())
+      {
+        topics.push_back(this->topic);
+      }
+      topics.push_back(std::string("/world/") + worldName + "/model/"
+        + this->model.Name(_ecm) + "/joint_state");
+
+      this->topic = validTopic(topics);
+      if (this->topic.empty())
+      {
+        ignerr << "No valid topics for JointStatePublisher could be found."
+          << "Make sure World/Model name does'nt contain invalid characters.\n";
+        return;
+      }
+
       this->modelPub = std::make_unique<transport::Node::Publisher>(
-          this->node.Advertise<msgs::Model>(topic));
+          this->node.Advertise<msgs::Model>(this->topic));
     }
   }
 
diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh
index 352137edae..75c13f15a7 100644
--- a/src/systems/joint_state_publisher/JointStatePublisher.hh
+++ b/src/systems/joint_state_publisher/JointStatePublisher.hh
@@ -20,6 +20,7 @@
 
 #include <memory>
 #include <set>
+#include <string>
 #include <ignition/gazebo/Model.hh>
 #include <ignition/transport/Node.hh>
 #include <ignition/gazebo/System.hh>
@@ -82,6 +83,9 @@ namespace systems
 
     /// \brief The joints that will be published.
     private: std::set<Entity> joints;
+
+    /// \brief The topic
+    private: std::string topic;
   };
   }
 }