From 582bc5de1a7dff4cdec2ad6e1a9f6f46b1530730 Mon Sep 17 00:00:00 2001
From: Carl Saldanha <cjds@users.noreply.github.com>
Date: Mon, 9 Aug 2021 12:01:28 -0700
Subject: [PATCH] Complaint if Joint doesn't exists before adding joint
 controller (#786)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: cjds <cjds@live.com>
Signed-off-by: root <cjds@live.com>
Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Co-authored-by: Louise Poubel <louise@openrobotics.org>
---
 .../joint_controller/JointController.cc       | 37 +++++++++----------
 test/integration/joint_controller_system.cc   | 19 ++++++++++
 test/worlds/joint_controller_invalid.sdf      | 14 +++++++
 3 files changed, 51 insertions(+), 19 deletions(-)
 create mode 100644 test/worlds/joint_controller_invalid.sdf

diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc
index 7de50652d1..3f5a1cdfd1 100644
--- a/src/systems/joint_controller/JointController.cc
+++ b/src/systems/joint_controller/JointController.cc
@@ -47,9 +47,6 @@ class ignition::gazebo::systems::JointControllerPrivate
   /// \brief Joint Entity
   public: Entity jointEntity;
 
-  /// \brief Joint name
-  public: std::string jointName;
-
   /// \brief Commanded joint velocity
   public: double jointVelCmd;
 
@@ -89,15 +86,23 @@ void JointController::Configure(const Entity &_entity,
   }
 
   // Get params from SDF
-  this->dataPtr->jointName = _sdf->Get<std::string>("joint_name");
-
-  if (this->dataPtr->jointName == "")
+  auto jointName = _sdf->Get<std::string>("joint_name");
+  if (jointName.empty())
   {
     ignerr << "JointController found an empty jointName parameter. "
            << "Failed to initialize.";
     return;
   }
 
+  this->dataPtr->jointEntity = this->dataPtr->model.JointByName(_ecm,
+      jointName);
+  if (this->dataPtr->jointEntity == kNullEntity)
+  {
+    ignerr << "Joint with name[" << jointName << "] not found. "
+    << "The JointController may not control this joint.\n";
+    return;
+  }
+
   if (_sdf->HasElement("initial_velocity"))
   {
     this->dataPtr->jointVelCmd = _sdf->Get<double>("initial_velocity");
@@ -139,11 +144,11 @@ void JointController::Configure(const Entity &_entity,
 
   // Subscribe to commands
   std::string topic = transport::TopicUtils::AsValidTopic("/model/" +
-      this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName +
+      this->dataPtr->model.Name(_ecm) + "/joint/" + jointName +
       "/cmd_vel");
   if (topic.empty())
   {
-    ignerr << "Failed to create topic for joint [" << this->dataPtr->jointName
+    ignerr << "Failed to create topic for joint [" << jointName
            << "]" << std::endl;
     return;
   }
@@ -155,7 +160,7 @@ void JointController::Configure(const Entity &_entity,
     if (topic.empty())
     {
       ignerr << "Failed to create topic [" << _sdf->Get<std::string>("topic")
-             << "]" << " for joint [" << this->dataPtr->jointName
+             << "]" << " for joint [" << jointName
              << "]" << std::endl;
       return;
     }
@@ -173,6 +178,10 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
 {
   IGN_PROFILE("JointController::PreUpdate");
 
+  // If the joint hasn't been identified yet, the plugin is disabled
+  if (this->dataPtr->jointEntity == kNullEntity)
+    return;
+
   // \TODO(anyone) Support rewind
   if (_info.dt < std::chrono::steady_clock::duration::zero())
   {
@@ -181,16 +190,6 @@ void JointController::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
         << "s]. System may not work properly." << std::endl;
   }
 
-  // If the joint hasn't been identified yet, look for it
-  if (this->dataPtr->jointEntity == kNullEntity)
-  {
-    this->dataPtr->jointEntity =
-        this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName);
-  }
-
-  if (this->dataPtr->jointEntity == kNullEntity)
-    return;
-
   // Nothing left to do if paused.
   if (_info.paused)
     return;
diff --git a/test/integration/joint_controller_system.cc b/test/integration/joint_controller_system.cc
index a773c404b2..8f280f9696 100644
--- a/test/integration/joint_controller_system.cc
+++ b/test/integration/joint_controller_system.cc
@@ -224,3 +224,22 @@ TEST_F(JointControllerTestFixture, JointVelocityCommandWithForce)
   EXPECT_NEAR(0, angularVelocity.Y(), 1e-2);
   EXPECT_NEAR(testAngVel, angularVelocity.Z(), 1e-2);
 }
+
+/////////////////////////////////////////////////
+TEST_F(JointControllerTestFixture, InexistentJoint)
+{
+  using namespace std::chrono_literals;
+
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "joint_controller_invalid.sdf");
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // Run some iterations to make sure nothing explodes
+  server.Run(true, 100, false);
+}
diff --git a/test/worlds/joint_controller_invalid.sdf b/test/worlds/joint_controller_invalid.sdf
new file mode 100644
index 0000000000..4621474f99
--- /dev/null
+++ b/test/worlds/joint_controller_invalid.sdf
@@ -0,0 +1,14 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="default">
+    <model name="joint_controller_test">
+      <link name="base_link">
+      </link>
+      <plugin
+        filename="ignition-gazebo-joint-controller-system"
+        name="ignition::gazebo::systems::JointController">
+        <joint_name>invalid</joint_name>
+      </plugin>
+    </model>
+  </world>
+</sdf>