Complaint if Joint doesn't exists before adding joint controller

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;

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);
+}

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>