fix(joint_locker): fix joint locker unlock bug (#212)

This commit ensures that the joint limits are not set to 0.0 when the
unlock feature is called before the lock feature.

panda_gazebo/src/panda_joint_locker_world_plugin.cpp

@@ -102,14 +102,18 @@ class PandaJointLockerPlugin : public WorldPlugin
       }
       else
       {
-        // Unlock the joint by restoring the old joint limits.
-        joint->SetLowerLimit(JOINT_AXIS_INDEX, this->oldLowerLimit[joint_name]);
-        joint->SetUpperLimit(JOINT_AXIS_INDEX, this->oldUpperLimit[joint_name]);
+        // Unlock the joint by restoring the old joint limits if they exist.
+        if (this->oldLowerLimit.find(joint_name) != this->oldLowerLimit.end() &&
+            this->oldUpperLimit.find(joint_name) != this->oldUpperLimit.end())
+        {
+          joint->SetLowerLimit(JOINT_AXIS_INDEX, this->oldLowerLimit[joint_name]);
+          joint->SetUpperLimit(JOINT_AXIS_INDEX, this->oldUpperLimit[joint_name]);
+        }
       }
     }
 
     res.success = true;
-    res.message = "Joints updated successfully";
+    res.message = "Joint " + (req.lock ? "locking" : "unlocking") + " successful";
     return true;
   }
 };